ravens/environment.py
def pick_place(self, pose0, pose1):
        """Execute pick and place primitive.

        Standard ravens tasks use the `delta` vector to lower the gripper
        until it makes contact with something. With deformables, however, we
        need to consider cases when the gripper could detect a rigid OR a
        soft body (cloth or bag); it should grip the first item it touches.
        This is handled in the Gripper class.

        Different deformable ravens tasks use slightly different parameters
        for better physics (and in some cases, faster simulation). Therefore,
        rather than make special cases here, those tasks will define their
        own action parameters, which we use here if they exist. Otherwise, we
        stick to defaults from standard ravens. Possible action parameters a
        task might adjust:

            speed: how fast the gripper moves.
            delta_z: how fast the gripper lowers for picking / placing.
            prepick_z: height of the gripper when it goes above the target
                pose for picking, just before it lowers.
            postpick_z: after suction gripping, raise to this height, should
                generally be low for cables / cloth.
            preplace_z: like prepick_z, but for the placing pose.
            pause_place: add a small pause for some tasks (e.g., bags) for
                slightly better soft body physics.
            final_z: height of the gripper after the action. Recommended to
                leave it at the default of 0.3, because it has to be set high
                enough to avoid the gripper occluding the workspace when
                generating color/depth maps.
        Args:
            pose0: picking pose.
            pose1: placing pose.

        Returns:
            A bool indicating whether the action succeeded or not, via
            checking the sequence of movep calls. If any movep failed, then
            self.step() will terminate the episode after this action.
        """
        # Defaults used in the standard Ravens environments.
        speed = 0.01
        delta_z = -0.001
        prepick_z = 0.3
        postpick_z = 0.3
        preplace_z = 0.3
        pause_place = 0.0
        final_z = 0.3

        # Find parameters, which may depend on the task stage.
        if hasattr(self.task, 'primitive_params'):
            ts = self.task.task_stage
            if 'prepick_z' in self.task.primitive_params[ts]:
                prepick_z = self.task.primitive_params[ts]['prepick_z']
            speed       = self.task.primitive_params[ts]['speed']
            delta_z     = self.task.primitive_params[ts]['delta_z']
            postpick_z  = self.task.primitive_params[ts]['postpick_z']
            preplace_z  = self.task.primitive_params[ts]['preplace_z']
            pause_place = self.task.primitive_params[ts]['pause_place']

        # Used to track deformable IDs, so that we can get the vertices.
        def_IDs = []
        if hasattr(self.task, 'def_IDs'):
            def_IDs = self.task.def_IDs

        # Otherwise, proceed as normal.
        success = True
        pick_position = np.array(pose0[0])
        pick_rotation = np.array(pose0[1])
        prepick_position = pick_position.copy()
        prepick_position[2] = prepick_z

        # Execute picking motion primitive.
        prepick_pose = np.hstack((prepick_position, pick_rotation))
        success &= self.movep(prepick_pose)
        target_pose = prepick_pose.copy()
        delta = np.array([0, 0, delta_z, 0, 0, 0, 0])

        # Lower gripper until (a) touch object (rigid OR softbody), or (b) hit ground.
        while not self.ee.detect_contact(def_IDs) and target_pose[2] > 0:
            target_pose += delta
            success &= self.movep(target_pose)

        # Create constraint (rigid objects) or anchor (deformable).
        self.ee.activate(self.objects, def_IDs)

        # Increase z slightly (or hard-code it) and check picking success.
        if self.is_softbody_env() or self.is_new_cable_env():
            prepick_pose[2] = postpick_z
            success &= self.movep(prepick_pose, speed=speed)
            time.sleep(pause_place) # extra rest for bags
        elif isinstance(self.task, tasks.names['cable']):
            prepick_pose[2] = 0.03
            success &= self.movep(prepick_pose, speed=0.001)
        else:
            prepick_pose[2] += pick_position[2]
            success &= self.movep(prepick_pose)
        pick_success = self.ee.check_grasp()

        if pick_success:
            place_position = np.array(pose1[0])
            place_rotation = np.array(pose1[1])
            preplace_position = place_position.copy()
            preplace_position[2] = 0.3 + pick_position[2]

            # Execute placing motion primitive if pick success.
            preplace_pose = np.hstack((preplace_position, place_rotation))
            if self.is_softbody_env() or self.is_new_cable_env():
                preplace_pose[2] = preplace_z
                success &= self.movep(preplace_pose, speed=speed)
                time.sleep(pause_place) # extra rest for bags
            elif isinstance(self.task, tasks.names['cable']):
                preplace_pose[2] = 0.03
                success &= self.movep(preplace_pose, speed=0.001)
            else:
                success &= self.movep(preplace_pose)

            # Lower the gripper. Here, we have a fixed speed=0.01. TODO: consider additional
            # testing with bags, so that the 'lowering' process for bags is more reliable.
            target_pose = preplace_pose.copy()
            while not self.ee.detect_contact(def_IDs) and target_pose[2] > 0:
                target_pose += delta
                success &= self.movep(target_pose)

            # Release AND get gripper high up, to clear the view for images.
            self.ee.release()
            preplace_pose[2] = final_z
            success &= self.movep(preplace_pose)
        else:
            # Release AND get gripper high up, to clear the view for images.
            self.ee.release()
            prepick_pose[2] = final_z
            success &= self.movep(prepick_pose)
        return success
Execute pick and place primitive.

        Standard ravens tasks use the `delta` vector to lower the gripper
        until it makes contact with something. With deformables, however, we
        need to consider cases when the gripper could detect a rigid OR a
        soft body (cloth or bag); it should grip the first item it touches.
        This is handled in the Gripper class.

        Different deformable ravens tasks use slightly different parameters
        for better physics (and in some cases, faster simulation). Therefore,
        rather than make special cases here, those tasks will define their
        own action parameters, which we use here if they exist. Otherwise, we
        stick to defaults from standard ravens. Possible action parameters a
        task might adjust:

            speed: how fast the gripper moves.
            delta_z: how fast the gripper lowers for picking / placing.
            prepick_z: height of the gripper when it goes above the target
                pose for picking, just before it lowers.
            postpick_z: after suction gripping, raise to this height, should
                generally be low for cables / cloth.
            preplace_z: like prepick_z, but for the placing pose.
            pause_place: add a small pause for some tasks (e.g., bags) for
                slightly better soft body physics.
            final_z: height of the gripper after the action. Recommended to
                leave it at the default of 0.3, because it has to be set high
                enough to avoid the gripper occluding the workspace when
                generating color/depth maps.
        Args:
            pose0: picking pose.
            pose1: placing pose.

        Returns:
            A bool indicating whether the action succeeded or not, via
            checking the sequence of movep calls. If any movep failed, then
            self.step() will terminate the episode after this action.
        
Execute pick and place primitive.
Standard ravens tasks use the `delta` vector to lower the gripper
until it makes contact with something. With deformables, however, we
need to consider cases when the gripper could detect a rigid OR a
soft body (cloth or bag); it should grip the first item it touches.
This is handled in the Gripper class.

Different deformable ravens tasks use slightly different parameters
for better physics (and in some cases, faster simulation). Therefore,
rather than make special cases here, those tasks will define their
own action parameters, which we use here if they exist. Otherwise, we
stick to defaults from standard ravens. Possible action parameters a
task might adjust.


Execute pick and place primitive . Standard ravens tasks use the ` delta ` vector to lower the gripper until it makes contact with something . With deformables however we need to consider cases when the gripper could detect a rigid OR a soft body ( cloth or bag ) ; it should grip the first item it touches . This is handled in the Gripper class . Different deformable ravens tasks use slightly different parameters for better physics ( and in some cases faster simulation ) . Therefore rather than make special cases here those tasks will define their own action parameters which we use here if they exist . Otherwise we stick to defaults from standard ravens . Possible action parameters a task might adjust .
def pick_place(self, pose0, pose1):
        speed = 0.01
        delta_z = -0.001
        prepick_z = 0.3
        postpick_z = 0.3
        preplace_z = 0.3
        pause_place = 0.0
        final_z = 0.3
        if hasattr(self.task, 'primitive_params'):
            ts = self.task.task_stage
            if 'prepick_z' in self.task.primitive_params[ts]:
                prepick_z = self.task.primitive_params[ts]['prepick_z']
            speed       = self.task.primitive_params[ts]['speed']
            delta_z     = self.task.primitive_params[ts]['delta_z']
            postpick_z  = self.task.primitive_params[ts]['postpick_z']
            preplace_z  = self.task.primitive_params[ts]['preplace_z']
            pause_place = self.task.primitive_params[ts]['pause_place']
        def_IDs = []
        if hasattr(self.task, 'def_IDs'):
            def_IDs = self.task.def_IDs
        success = True
        pick_position = np.array(pose0[0])
        pick_rotation = np.array(pose0[1])
        prepick_position = pick_position.copy()
        prepick_position[2] = prepick_z
        prepick_pose = np.hstack((prepick_position, pick_rotation))
        success &= self.movep(prepick_pose)
        target_pose = prepick_pose.copy()
        delta = np.array([0, 0, delta_z, 0, 0, 0, 0])
        while not self.ee.detect_contact(def_IDs) and target_pose[2] > 0:
            target_pose += delta
            success &= self.movep(target_pose)
        self.ee.activate(self.objects, def_IDs)
        if self.is_softbody_env() or self.is_new_cable_env():
            prepick_pose[2] = postpick_z
            success &= self.movep(prepick_pose, speed=speed)
            time.sleep(pause_place) 
        elif isinstance(self.task, tasks.names['cable']):
            prepick_pose[2] = 0.03
            success &= self.movep(prepick_pose, speed=0.001)
        else:
            prepick_pose[2] += pick_position[2]
            success &= self.movep(prepick_pose)
        pick_success = self.ee.check_grasp()
        if pick_success:
            place_position = np.array(pose1[0])
            place_rotation = np.array(pose1[1])
            preplace_position = place_position.copy()
            preplace_position[2] = 0.3 + pick_position[2]
            preplace_pose = np.hstack((preplace_position, place_rotation))
            if self.is_softbody_env() or self.is_new_cable_env():
                preplace_pose[2] = preplace_z
                success &= self.movep(preplace_pose, speed=speed)
                time.sleep(pause_place) 
            elif isinstance(self.task, tasks.names['cable']):
                preplace_pose[2] = 0.03
                success &= self.movep(preplace_pose, speed=0.001)
            else:
                success &= self.movep(preplace_pose)
            target_pose = preplace_pose.copy()
            while not self.ee.detect_contact(def_IDs) and target_pose[2] > 0:
                target_pose += delta
                success &= self.movep(target_pose)
            self.ee.release()
            preplace_pose[2] = final_z
            success &= self.movep(preplace_pose)
        else:
            self.ee.release()
            prepick_pose[2] = final_z
            success &= self.movep(prepick_pose)
        return success
def pick_place ( self , pose0 , pose1 ) : speed = 0.01 delta_z = - 0.001 prepick_z = 0.3 postpick_z = 0.3 preplace_z = 0.3 pause_place = 0.0 final_z = 0.3 if hasattr ( self . task , 'primitive_params' ) : ts = self . task . task_stage if 'prepick_z' in self . task . primitive_params [ ts ] : prepick_z = self . task . primitive_params [ ts ] [ 'prepick_z' ] speed = self . task . primitive_params [ ts ] [ 'speed' ] delta_z = self . task . primitive_params [ ts ] [ 'delta_z' ] postpick_z = self . task . primitive_params [ ts ] [ 'postpick_z' ] preplace_z = self . task . primitive_params [ ts ] [ 'preplace_z' ] pause_place = self . task . primitive_params [ ts ] [ 'pause_place' ] def_IDs = [ ] if hasattr ( self . task , 'def_IDs' ) : def_IDs = self . task . def_IDs success = True pick_position = np . array ( pose0 [ 0 ] ) pick_rotation = np . array ( pose0 [ 1 ] ) prepick_position = pick_position . copy ( ) prepick_position [ 2 ] = prepick_z prepick_pose = np . hstack ( ( prepick_position , pick_rotation ) ) success &= self . movep ( prepick_pose ) target_pose = prepick_pose . copy ( ) delta = np . array ( [ 0 , 0 , delta_z , 0 , 0 , 0 , 0 ] ) while not self . ee . detect_contact ( def_IDs ) and target_pose [ 2 ] > 0 : target_pose += delta success &= self . movep ( target_pose ) self . ee . activate ( self . objects , def_IDs ) if self . is_softbody_env ( ) or self . is_new_cable_env ( ) : prepick_pose [ 2 ] = postpick_z success &= self . movep ( prepick_pose , speed = speed ) time . sleep ( pause_place ) elif isinstance ( self . task , tasks . names [ 'cable' ] ) : prepick_pose [ 2 ] = 0.03 success &= self . movep ( prepick_pose , speed = 0.001 ) else : prepick_pose [ 2 ] += pick_position [ 2 ] success &= self . movep ( prepick_pose ) pick_success = self . ee . check_grasp ( ) if pick_success : place_position = np . array ( pose1 [ 0 ] ) place_rotation = np . array ( pose1 [ 1 ] ) preplace_position = place_position . copy ( ) preplace_position [ 2 ] = 0.3 + pick_position [ 2 ] preplace_pose = np . hstack ( ( preplace_position , place_rotation ) ) if self . is_softbody_env ( ) or self . is_new_cable_env ( ) : preplace_pose [ 2 ] = preplace_z success &= self . movep ( preplace_pose , speed = speed ) time . sleep ( pause_place ) elif isinstance ( self . task , tasks . names [ 'cable' ] ) : preplace_pose [ 2 ] = 0.03 success &= self . movep ( preplace_pose , speed = 0.001 ) else : success &= self . movep ( preplace_pose ) target_pose = preplace_pose . copy ( ) while not self . ee . detect_contact ( def_IDs ) and target_pose [ 2 ] > 0 : target_pose += delta success &= self . movep ( target_pose ) self . ee . release ( ) preplace_pose [ 2 ] = final_z success &= self . movep ( preplace_pose ) else : self . ee . release ( ) prepick_pose [ 2 ] = final_z success &= self . movep ( prepick_pose ) return success
Execute pick and place primitive.
Execute pick and place primitive .
"""Execute pick and place primitive.

        Standard ravens tasks use the `delta` vector to lower the gripper
        until it makes contact with something. With deformables, however, we
        need to consider cases when the gripper could detect a rigid OR a
        soft body (cloth or bag); it should grip the first item it touches.
        This is handled in the Gripper class.

        Different deformable ravens tasks use slightly different parameters
        for better physics (and in some cases, faster simulation). Therefore,
        rather than make special cases here, those tasks will define their
        own action parameters, which we use here if they exist. Otherwise, we
        stick to defaults from standard ravens. Possible action parameters a
        task might adjust:

            speed: how fast the gripper moves.
            delta_z: how fast the gripper lowers for picking / placing.
            prepick_z: height of the gripper when it goes above the target
                pose for picking, just before it lowers.
            postpick_z: after suction gripping, raise to this height, should
                generally be low for cables / cloth.
            preplace_z: like prepick_z, but for the placing pose.
            pause_place: add a small pause for some tasks (e.g., bags) for
                slightly better soft body physics.
            final_z: height of the gripper after the action. Recommended to
                leave it at the default of 0.3, because it has to be set high
                enough to avoid the gripper occluding the workspace when
                generating color/depth maps.
        Args:
            pose0: picking pose.
            pose1: placing pose.

        Returns:
            A bool indicating whether the action succeeded or not, via
            checking the sequence of movep calls. If any movep failed, then
            self.step() will terminate the episode after this action.
        """ # Defaults used in the standard Ravens environments. # Find parameters, which may depend on the task stage. # Used to track deformable IDs, so that we can get the vertices. # Otherwise, proceed as normal. # Execute picking motion primitive. # Lower gripper until (a) touch object (rigid OR softbody), or (b) hit ground. # Create constraint (rigid objects) or anchor (deformable). # Increase z slightly (or hard-code it) and check picking success. # extra rest for bags # Execute placing motion primitive if pick success. # extra rest for bags # Lower the gripper. Here, we have a fixed speed=0.01. TODO: consider additional # testing with bags, so that the 'lowering' process for bags is more reliable. # Release AND get gripper high up, to clear the view for images. # Release AND get gripper high up, to clear the view for images.
A bool indicating whether the action succeeded or not, via
checking the sequence of movep calls. If any movep failed, then
self.step() will terminate the episode after this action.
A bool indicating whether the action succeeded or not via checking the sequence of movep calls . If any movep failed then self . step () will terminate the episode after this action .
self
pose0
None
pose1
None