Set speed scaling factor for ApproachMotion/LiftMotion in pymoveit_mtc.stages.Pick

[matthias88]

Hi all,

unfortunately I didn't find a solution to change cartesian speed (via velocity and acceleration scaling factor) for the approach and lift motion in the pick stage. In my previous attempt I used the move relative stage, which accepts the settings from the cartesian interpolation planner.

Previous attempt:

        # Cartesian interpolation planner
        cartesian = core.CartesianPath()
        cartesian.max_acceleration_scaling_factor = scaling_factor_speed
        cartesian.max_velocity_scaling_factor = scaling_factor_speed
        cartesian.step_size = 0.001
        cartesian.precision.translational = 0.001
...
        move = stages.MoveRelative("z +0.02", cartesian)
        move.group = static_params.get('group_name_arm')
        move.setDirection(
            Vector3Stamped(
                header=Header(frame_id=static_params.get('world_frame')),
                vector=Vector3(x=0.0, y=0.0, z=+0.02),
            )
        )
        self.task.add(move)

Current attempt using pick container for a more general task (code refactoring):

        pick = stages.Pick(simpleGrasp, "Pick")
        pick.eef = "gripper"
        pick.eef_frame = "tcp"
        pick.object = "grasp_object_0"

        # Twist to approach the object
        approach = TwistStamped()
        approach.header.frame_id = "tcp"
        approach.twist.linear.z = 1.0
        pick.setApproachMotion(approach, 0.02, 0.03)

        # Twist to lift the object
        lift = TwistStamped()
        lift.header.frame_id = "tcp"
        lift.twist.linear.z = -1.0
        pick.setLiftMotion(lift, 0.02, 0.03)

        # Add the pick stage to the task's stage hierarchy
        self.task.add(pick)    

It seems, that also the limit parameters for speed scaling in joint_limits.yaml are completely ignored. Is there a simple solution/workaround for that issue?

[rhaschke]

Indeed, there is currently no solution available to set the speed scaling factors. I just tried to fix that but ran into a segfault. I will need more time to look deeper...

[matthias88]

Thank you for the quick review and feedback. My first attempt to work around this issue was to replace the pick/place container with a serial container reconstructing the pick container with similar behaviour, but there are some other issues like for the place container (due to missing props/attributes?):
Failing stage(s):
open gripper (0/6): undefined goal.

        multiplicator = task_request.get('multiplicator')
        
        # The grasp generator spawns a set of possible grasp poses around the object
        grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
        grasp_generator.angle_delta = math.pi / multiplicator
        grasp_generator.pregrasp = "open"
        grasp_generator.grasp = "close"
        grasp_generator.setMonitoredStage(self.task["add poses"])  # Generate solutions for all initial states

       # SimpleGrasp container encapsulates IK calculation of arm pose as well as finger closing
        simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp")
        
        # Set frame for IK calculation (create pose wrt. the origin of the tcp)
        ik_frame = PoseStamped()
        ik_frame.header.frame_id = "tcp" #tcp
        top_down_orientation = pq(axis=[1, 0, 0], angle=math.pi)  # 180 degrees around x-axis        
        ik_frame.pose.orientation.x = top_down_orientation.x
        ik_frame.pose.orientation.y = top_down_orientation.y
        ik_frame.pose.orientation.z = top_down_orientation.z
        ik_frame.pose.orientation.w = top_down_orientation.w        
        simpleGrasp.setIKFrame(ik_frame)
        simpleGrasp.object = "grasp_object_0"
        simpleGrasp.eef = "gripper"

        # Approach motion (MoveRelative)
        approach_stage = stages.MoveRelative("approach object", cartesian)
        approach_stage.group = static_params.get('group_name_arm')
        approach_stage.setDirection(
            Vector3Stamped(
                header=Header(frame_id=static_params.get('gripper_frame')),
                vector=Vector3(x=0.0, y=0.0, z=0.02),
            )
        )
        # Lift motion (MoveRelative)
        lift_stage = stages.MoveRelative("lift object", cartesian)
        lift_stage.group = static_params.get('group_name_arm')
        lift_stage.setDirection(
            Vector3Stamped(
                header=Header(frame_id=static_params.get('gripper_frame')),
                vector=Vector3(x=0.0, y=0.0, z=-0.02),
            )
        )  
        
        # Create serial container and add stages
        pick_container = core.SerialContainer("Pick")
        pick_container.add(approach_stage)
        pick_container.add(simpleGrasp)
        pick_container.add(lift_stage)
 
        self.task.add(pick_container)

        connect = stages.Connect("connect1", planners)
        connect.timeout = 0.2
        # connect.path_constraints = constraints
        self.task.add(connect)

        # move to deploy pos
        placePose = PoseStamped()
        placePose.header.frame_id = "nest_1"
        placePose.pose = pose_goal

        # Generate Cartesian place poses for the object
        place_generator = stages.GeneratePlacePose("Generate Place Pose")
        place_generator.setMonitoredStage(self.task["Pick"])
        place_generator.object = "grasp_object_0"
        place_generator.pose = placePose

        # The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
        simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
        simpleUnGrasp.object = "grasp_object_0"
        simpleUnGrasp.eef = "gripper"

        # Place motion (MoveRelative)
        place_stage = stages.MoveRelative("place object", cartesian)
        place_stage.group = static_params.get('group_name_arm')
        place_stage.setDirection(
            Vector3Stamped(
                header=Header(frame_id=static_params.get('gripper_frame')),
                vector=Vector3(x=0.0, y=0.0, z=0.02),
            )
        )
        # Retract motion (MoveRelative)
        retract_stage = stages.MoveRelative("retract object", cartesian)
        retract_stage.group = static_params.get('group_name_arm')
        retract_stage.setDirection(
            Vector3Stamped(
                header=Header(frame_id=static_params.get('gripper_frame')),
                vector=Vector3(x=0.0, y=0.0, z=-0.02),
            )
        )  
        
        # Create serial container and add stages
        place_container = core.SerialContainer("Place")
        place_container.add(place_stage)
        place_container.add(simpleUnGrasp)
        place_container.add(retract_stage)
 
        self.task.add(place_container)

[rhaschke]

From your first code snippet, I was concluding that you are constrained to Python. In C++ you can easily change the scaling factors like this:

auto solver = pick.cartesianSolver()
solver->setProperty("max_velocity_scaling_factor", 0.1)
solver->setProperty("max_acceleration_scaling_factor", 0.1)

Note that they apply to both, approaching and lifting. If you need more fine-grained control, you need to rebuild the Pick container.
Have a look at the current implementation for inspiration: pick.cpp and its use, e.g. in pick_ur5.cpp.

Your failing grasp stage is probably due to a missing setPreGraspPose:

moveit_task_constructor/core/test/pick_ur5.cpp

Line 55 in bad8e13

grasp_generator->setPreGraspPose("open");

[matthias88]

Thanks for your inspiration 👍 I will have a look at it and try to modify the example above with correctly forwarding the necessary pregrasp property