From 2af8da68b0b65185af2f9018146647c757fd6332 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Tue, 26 Sep 2023 16:36:20 -0600 Subject: [PATCH 01/10] Update ros2_kortex dependency repo Point the ros2_kortex repo to the latest commit to pull in the updated kinova drivers. --- .gitmodules | 4 ++-- dependencies/ros2_kortex | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index 222f03f..71a70fc 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "dependencies/ros2_kortex"] path = dependencies/ros2_kortex - url = https://github.com/PickNikRobotics/ros2_kortex.git - branch = 04-24-2023-freeze + url = https://github.com/Kinovarobotics/ros2_kortex.git + branch = main diff --git a/dependencies/ros2_kortex b/dependencies/ros2_kortex index 454e89f..a2f018a 160000 --- a/dependencies/ros2_kortex +++ b/dependencies/ros2_kortex @@ -1 +1 @@ -Subproject commit 454e89fa1fd3acd8141a5518f8e1dc6f4bc03121 +Subproject commit a2f018ab8c39e97fe91f57c27b8e59852b029da4 From 0179bc433d0ebf0f36c22f53a170bf8c1263a8b0 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Tue, 26 Sep 2023 17:02:29 -0600 Subject: [PATCH 02/10] Use correct joint names in ompl_planning config The projection_evaluator ompl config was set to joint names for the ur5. This changes that setting to the equivalent (first 2) joints of the kinova. --- .../config/moveit/ompl_planning.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/picknik_kinova_gen3_base_config/config/moveit/ompl_planning.yaml b/src/picknik_kinova_gen3_base_config/config/moveit/ompl_planning.yaml index 14b496f..2493f41 100644 --- a/src/picknik_kinova_gen3_base_config/config/moveit/ompl_planning.yaml +++ b/src/picknik_kinova_gen3_base_config/config/moveit/ompl_planning.yaml @@ -68,7 +68,7 @@ manipulator: - PRMstarkConfigDefault enforce_constrained_state_space: true #Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE - projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + projection_evaluator: joints(joint_1,joint_2) longest_valid_segment_fraction: 0.01 gripper: planner_configs: From 754bdbfae2416cf521c5cf56e1a5f87458dcf2a2 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Tue, 26 Sep 2023 17:06:13 -0600 Subject: [PATCH 03/10] Update kinova srdf The old srdf was outdated and had incorrect joint and link names for the robotiq gripper. This updated version is based on https://github.com/PickNikRobotics/AWS_skills_center_config/blob/8351394e2eb359550b31b0e3704ae21f81984704/kinova_gen3_picknik_config/config/moveit/picknik_kinova_gen3.srdf --- .../moveit/picknik_kinova_gen3_base.srdf | 313 +++++++----------- 1 file changed, 125 insertions(+), 188 deletions(-) diff --git a/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf b/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf index 80772e1..939ecb3 100644 --- a/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf +++ b/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf @@ -10,221 +10,158 @@ - - - - - - - - - + - - - - - - - - - - - + + + + + + + + + + + + + - - - - - - + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - - - - + + + + - - - - - - - - - - - - - - + + + + + + + + + + - - + - - - - - - - - - - - + + + + + + + + + - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - From ce7a29db90faf5b652a3d2d17eb859382e7cf335 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 27 Sep 2023 09:55:15 -0600 Subject: [PATCH 04/10] Update planning configs Add pilz cartesian planner limits (fixes waypoint manual control) Add STOMP planning configuration (fixes Move To Waypoint behavior with use_all_planners set to True) --- .../config/base_config.yaml | 6 ++++++ .../config/moveit/pilz_cartesian_limits.yaml | 6 ++++++ .../config/moveit/stomp_planning.yaml | 18 ++++++++++++++++++ 3 files changed, 30 insertions(+) create mode 100644 src/picknik_kinova_gen3_base_config/config/moveit/pilz_cartesian_limits.yaml create mode 100644 src/picknik_kinova_gen3_base_config/config/moveit/stomp_planning.yaml diff --git a/src/picknik_kinova_gen3_base_config/config/base_config.yaml b/src/picknik_kinova_gen3_base_config/config/base_config.yaml index 174e0a3..eb9c1fe 100644 --- a/src/picknik_kinova_gen3_base_config/config/base_config.yaml +++ b/src/picknik_kinova_gen3_base_config/config/base_config.yaml @@ -61,6 +61,9 @@ moveit_params: ompl_planning: package: "picknik_kinova_gen3_base_config" path: "config/moveit/ompl_planning.yaml" + stomp_planning: + package: "picknik_kinova_gen3_base_config" + path: "config/moveit/stomp_planning.yaml" kinematics: package: "picknik_kinova_gen3_base_config" path: "config/moveit/kinematics.yaml" @@ -73,6 +76,9 @@ moveit_params: joint_limits: package: "picknik_kinova_gen3_base_config" path: "config/moveit/joint_limits.yaml" + pilz_cartesian_limits: + package: "picknik_kinova_gen3_base_config" + path: "config/moveit/pilz_cartesian_limits.yaml" publish: planning_scene: True diff --git a/src/picknik_kinova_gen3_base_config/config/moveit/pilz_cartesian_limits.yaml b/src/picknik_kinova_gen3_base_config/config/moveit/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/src/picknik_kinova_gen3_base_config/config/moveit/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/src/picknik_kinova_gen3_base_config/config/moveit/stomp_planning.yaml b/src/picknik_kinova_gen3_base_config/config/moveit/stomp_planning.yaml new file mode 100644 index 0000000..47d3115 --- /dev/null +++ b/src/picknik_kinova_gen3_base_config/config/moveit/stomp_planning.yaml @@ -0,0 +1,18 @@ +planning_plugin: stomp_moveit/StompPlanner +request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints + +stomp_moveit: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + exponentiated_cost_sensitivity: 0.8 + control_cost_weight: 0.1 + delta_t: 0.1 From de6fd19c89b6557b96b6e668faf02bd5e1c9ae5a Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 27 Sep 2023 10:25:27 -0600 Subject: [PATCH 05/10] Set use_internal_bus_gripper_comm to false Otherwise, we initialize 2 different control methods for the robotiq gripper in the urdf. This causes an issue in the mocking of the gripper hardware and only one finger of the gripper moves in simulation. --- src/picknik_kinova_gen3_base_config/config/base_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/picknik_kinova_gen3_base_config/config/base_config.yaml b/src/picknik_kinova_gen3_base_config/config/base_config.yaml index eb9c1fe..fd9f2b1 100644 --- a/src/picknik_kinova_gen3_base_config/config/base_config.yaml +++ b/src/picknik_kinova_gen3_base_config/config/base_config.yaml @@ -47,7 +47,7 @@ hardware: - fake_sensor_commands: "false" - external_camera: "true" - wrist_realsense: "true" - - use_internal_bus_gripper_comm: "true" + - use_internal_bus_gripper_comm: "false" optional_feature_params: use_joystick: False From ba7e0425a1b1b99692bd63fd816976701c6454e5 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 27 Sep 2023 11:36:10 -0600 Subject: [PATCH 06/10] Update kinova site_config xacro and base srdf Add updated arguments to kinova site_config xacros so the model can be loaded using the latest xacro macros from the ros2_kortex repo. Always add the manual_grasp_link with the end_effector_link as the parent. Before, it was added with the tool_frame link as its parent link which broke the description sometimes since the tool_frame link was only included if the gripper argument was empty. Add in disable_collision tags between the robotiq links and the wrist camera links in the base_config srdf. Without these additions, planning always fails. --- .../moveit/picknik_kinova_gen3_base.srdf | 32 ++++++++++ .../description/picknik_kinova_gen3.xacro | 58 ++++++++++++++----- 2 files changed, 76 insertions(+), 14 deletions(-) diff --git a/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf b/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf index 939ecb3..26d8276 100644 --- a/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf +++ b/src/picknik_kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.srdf @@ -85,6 +85,8 @@ + + @@ -98,9 +100,14 @@ + + + + + @@ -118,6 +125,8 @@ + + @@ -127,6 +136,8 @@ + + @@ -135,6 +146,8 @@ + + @@ -142,26 +155,45 @@ + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro b/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro index bfd81a1..8003300 100644 --- a/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro +++ b/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro @@ -1,13 +1,12 @@ - - + @@ -18,6 +17,12 @@ + + + + + + @@ -33,8 +38,10 @@ - + fake_sensor_commands="$(arg fake_sensor_commands)" + sim_gazebo="$(arg sim_gazebo)" + sim_ignition="$(arg sim_ignition)" + sim_isaac="$(arg sim_isaac)" + use_external_cable="$(arg use_external_cable)" + gripper_max_velocity="$(arg gripper_joint_name)" + gripper_max_force="$(arg gripper_max_force)" > + @@ -95,17 +107,35 @@ + - - - + + + + + + + + + + + + + + + + + - - - - - - + + + + + + + + + From 5a09ba0c665b1c0170d3b0ba46a6add5ce52a934 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 27 Sep 2023 11:49:44 -0600 Subject: [PATCH 07/10] Update kinova base_config control config Add the streaming_controller back in and change it to joint-based control rather than twist-based control. Without the streaming controller defined in the control config an Error is printed to the log on studio startup since it is referenced in the base_config.yaml file Update the fault_controller type. Update the robotiq_gripper_controller joint parameter to the correct joint. --- .../picknik_kinova_gen3.ros2_control.yaml | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/picknik_kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml b/src/picknik_kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml index a112435..aaa55ac 100644 --- a/src/picknik_kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml +++ b/src/picknik_kinova_gen3_base_config/config/control/picknik_kinova_gen3.ros2_control.yaml @@ -8,17 +8,15 @@ controller_manager: joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController - # TODO: Currently the adaptive switching of controllers does not happen. - # Therefore, this part is commented to ensure the switching of controllers does not break ros2_control - # streaming_controller: - # type: kortex2_controllers/TwistController + streaming_controller: + type: position_controllers/JointGroupPositionController # The gripper controller just relays position commands robotiq_gripper_controller: type: position_controllers/GripperActionController fault_controller: - type: kortex2_controllers/FaultController + type: picknik_reset_fault_controller/PicknikResetFaultController joint_trajectory_controller: ros__parameters: @@ -59,17 +57,21 @@ joint_trajectory_controller: streaming_controller: ros__parameters: - joint: tcp - interface_names: - - twist.linear.x - - twist.linear.y - - twist.linear.z - - twist.angular.x - - twist.angular.y - - twist.angular.z + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - joint_7 + command_interfaces: + - position + state_interfaces: + - position robotiq_gripper_controller: ros__parameters: default: true - joint: finger_joint + joint: robotiq_85_left_knuckle_joint allow_stalling: true From c737956b80958e8d0ff0f785d69020a67df2c8a1 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 27 Sep 2023 11:56:37 -0600 Subject: [PATCH 08/10] Update the site_config 3 Waypoint Pick and Place Update the objective to use the updated MoveToWaypoint behavior instead of the old MoveToJointState behavior. --- .../objectives/3_waypoint_pick_and_place.xml | 56 +++++++++++-------- 1 file changed, 32 insertions(+), 24 deletions(-) diff --git a/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml index da23a96..d27c64e 100644 --- a/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml +++ b/src/picknik_kinova_gen3_site_config/objectives/3_waypoint_pick_and_place.xml @@ -1,27 +1,35 @@ - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 1ad2b88385e54b6dc121bb1ec4ebec8d8c58620f Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 27 Sep 2023 13:57:53 -0600 Subject: [PATCH 09/10] Fix iMarker manual control Update the end_effector link from the deprecated "tool_frame" to "manual_grasp_link" in servo/moveit configs and other places. --- src/picknik_kinova_gen3_base_config/config/base_config.yaml | 2 +- .../config/moveit/kinova_gen3_servo.yaml | 2 +- .../objectives/open_lever_handle_door_config.yaml | 2 +- .../description/picknik_kinova_gen3.xacro | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/picknik_kinova_gen3_base_config/config/base_config.yaml b/src/picknik_kinova_gen3_base_config/config/base_config.yaml index fd9f2b1..6ea822d 100644 --- a/src/picknik_kinova_gen3_base_config/config/base_config.yaml +++ b/src/picknik_kinova_gen3_base_config/config/base_config.yaml @@ -93,7 +93,7 @@ moveit_params: allowed_start_tolerance: 0.01 ui_params: - servo_endpoint_frame_id: "tool_frame" + servo_endpoint_frame_id: "manual_grasp_link" # This configures what controllers gets run at startup ros2_control: diff --git a/src/picknik_kinova_gen3_base_config/config/moveit/kinova_gen3_servo.yaml b/src/picknik_kinova_gen3_base_config/config/moveit/kinova_gen3_servo.yaml index cfa5a70..65f1cba 100644 --- a/src/picknik_kinova_gen3_base_config/config/moveit/kinova_gen3_servo.yaml +++ b/src/picknik_kinova_gen3_base_config/config/moveit/kinova_gen3_servo.yaml @@ -37,7 +37,7 @@ planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'wo is_primary_planning_scene_monitor: false # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there. ## Other frames -ee_frame_name: tool_frame # The name of the end effector link, used to return the EE pose +ee_frame_name: manual_grasp_link # The name of the end effector link, used to return the EE pose robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour diff --git a/src/picknik_kinova_gen3_base_config/objectives/open_lever_handle_door_config.yaml b/src/picknik_kinova_gen3_base_config/objectives/open_lever_handle_door_config.yaml index 25129d4..91c2dc1 100644 --- a/src/picknik_kinova_gen3_base_config/objectives/open_lever_handle_door_config.yaml +++ b/src/picknik_kinova_gen3_base_config/objectives/open_lever_handle_door_config.yaml @@ -7,7 +7,7 @@ GetDoorHandle: SetupMTCOpenLeverHandleDoor: arm_group_name: "manipulator" end_effector_group_name: "gripper" - ik_frame_name: "tool_frame" + ik_frame_name: "manual_grasp_link" # The hand should be placed 75% of the way down the handle length when # manipulating the handle. Valid value range [0, 1.0]. diff --git a/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro b/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro index 8003300..759038b 100644 --- a/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro +++ b/src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro @@ -128,7 +128,7 @@ - + From 652e4e093ede2f4541f07471c90531de0bc4c19e Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 27 Sep 2023 14:07:11 -0600 Subject: [PATCH 10/10] Update kinova base_config xacro Use updated xacro description file for the kinova_gen3_base_config package so it can load properly using the updated ros2_kortex changes --- .../description/picknik_kinova_gen3.xacro | 60 ++++++++++++++----- 1 file changed, 45 insertions(+), 15 deletions(-) diff --git a/src/picknik_kinova_gen3_base_config/description/picknik_kinova_gen3.xacro b/src/picknik_kinova_gen3_base_config/description/picknik_kinova_gen3.xacro index cd22658..759038b 100644 --- a/src/picknik_kinova_gen3_base_config/description/picknik_kinova_gen3.xacro +++ b/src/picknik_kinova_gen3_base_config/description/picknik_kinova_gen3.xacro @@ -1,13 +1,12 @@ - - + @@ -18,6 +17,12 @@ + + + + + + @@ -33,8 +38,10 @@ - + fake_sensor_commands="$(arg fake_sensor_commands)" + sim_gazebo="$(arg sim_gazebo)" + sim_ignition="$(arg sim_ignition)" + sim_isaac="$(arg sim_isaac)" + use_external_cable="$(arg use_external_cable)" + gripper_max_velocity="$(arg gripper_joint_name)" + gripper_max_force="$(arg gripper_max_force)" > + - + @@ -95,17 +107,35 @@ + - - - + + + + + + + + + + + + + + + + + - - - - - - + + + + + + + + +