The PR adds various parameters that allow users to control the behavior of cartesian planning. This includes:
Allow users to change the avoid_collisions, jump_threshold, prismatic_jump_threshold, and revolute_jump_threshold attributes of the cartesian path service call.
Allow users to specify what fraction of the path must be completed in order to accept the solution.
Allow users to set the max_step from _plan_async(…)
Allow users to access the robot’s end effector name and base link name.
[x] Largest max_step: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_max_step:=0.3, verify it succeeds
[x] Reset
[x] Text cartesian_jump_threshold:
[x] Small jump threshold: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=0.001, verify that the planning/execution call succeeds but the arm does not actually move.
[x] Reset
[x] Medium jump threshold: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=1.0, verify that it gets almost to the goal but not quite.
[x] Reset
[x] Large jump threshold: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=2.0, verify that it gets to the goal.
[x] Reset
[x] Test cartesian_fraction_threshold:
[x] High threshold that can be reached: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_fraction_threshold:=1.0, verify it succeeds
[x] Reset
[x] High threshold that cannot be reached: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=1.0 -p cartesian_fraction_threshold:=1.0, verify it fails
[x] Test without a collision object: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_avoid_collisions:=True, verify it succeeds
[x] Reset
[x] Add a collision object: ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="sphere" -p position:="[0.573, 0.000, 0.488]" -p dimensions:="[0.04]"
[x] Test with a collision object: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_avoid_collisions:=True, verify it stops before the collision
[x] Reset
[x] Test without avoiding collisions: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_avoid_collisions:=False, verify it goes all the way to the goal, ignoring the collision.
Description
The PR adds various parameters that allow users to control the behavior of cartesian planning. This includes:
avoid_collisions,jump_threshold,prismatic_jump_threshold, andrevolute_jump_thresholdattributes of the cartesian path service call.max_stepfrom_plan_async(…)Note that unfortunately, MoveIt2’s ROS interface does not currently support revolute or prismatic jump thresholds.
Testing
ros2 launch panda_moveit_config ex_fake_control.launch.pymax_step:ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True, verify it succeedsros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[0.0, -0.7853981633974483, 0.0, -2.356194490192345, 0.0, 1.5707963267948966, 0.7853981633974483]"ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_max_step:=0.0, verify it failsros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_max_step:=0.3, verify it succeedscartesian_jump_threshold:ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=0.001, verify that the planning/execution call succeeds but the arm does not actually move.ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=1.0, verify that it gets almost to the goal but not quite.ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=2.0, verify that it gets to the goal.cartesian_fraction_threshold:ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_fraction_threshold:=1.0, verify it succeedsros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_jump_threshold:=1.0 -p cartesian_fraction_threshold:=1.0, verify it failsros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_fraction_threshold:=1.1, verify it failscartesian_avoid_collisions:ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_avoid_collisions:=True, verify it succeedsros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="sphere" -p position:="[0.573, 0.000, 0.488]" -p dimensions:="[0.04]"ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_avoid_collisions:=True, verify it stops before the collisionros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.573, 0.000, 0.488]" -p quat_xyzw:="[1.000, -0.000, 0.001, -0.000]" -p cartesian:=True -p cartesian_avoid_collisions:=False, verify it goes all the way to the goal, ignoring the collision.