Merge pull request #7 from PickNikRobotics/kinova-site-config-updates

Kinova config updates

.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

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
@@ -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
@@ -87,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:

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

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

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:

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

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

src/picknik_kinova_gen3_base_config/description/picknik_kinova_gen3.xacro

@@ -1,13 +1,12 @@
 <?xml version="1.0"?>
-
 <robot name="gen3" xmlns:xacro="http://ros.org/wiki/xacro">
-
     <!-- Arguments for Kinova -->
     <xacro:arg name="arm" default="gen3" />
     <xacro:arg name="dof" default="7" />
     <xacro:arg name="vision" default="false" />
     <xacro:arg name="robot_ip" default="yyy.yyy.yyy.yyy" />
     <xacro:arg name="gripper" default="" />
+    <xacro:arg name="gripper_joint_name" default="robotiq_85_left_knuckle_joint" />
     <xacro:arg name="username" default="admin" />
     <xacro:arg name="password" default="admin" />
     <xacro:arg name="port" default="10000" />
@@ -18,6 +17,12 @@
     <xacro:arg name="prefix" default="" />
     <xacro:arg name="use_fake_hardware" default="false" />
     <xacro:arg name="fake_sensor_commands" default="false" />
+    <xacro:arg name="sim_gazebo" default="false" />
+    <xacro:arg name="sim_ignition" default="false" />
+    <xacro:arg name="sim_isaac" default="false" />
+    <xacro:arg name="use_external_cable" default="false" />
+    <xacro:arg name="gripper_max_velocity" default="100.0" />
+    <xacro:arg name="gripper_max_force" default="100.0" />
     <xacro:arg name="wrist_realsense" default="true" />
 
     <!-- Import macros for main hardware components -->
@@ -33,8 +38,10 @@
     <!-- Load the robot -->
     <xacro:load_robot
       parent="world"
+      prefix="$(arg prefix)"
       arm="$(arg arm)"
       gripper="$(arg gripper)"
+      gripper_joint_name="$(arg gripper_joint_name)"
       dof="$(arg dof)"
       vision="$(arg vision)"
       robot_ip="$(arg robot_ip)"
@@ -45,15 +52,20 @@
       session_inactivity_timeout_ms="$(arg session_inactivity_timeout_ms)"
       connection_inactivity_timeout_ms="$(arg connection_inactivity_timeout_ms)"
       use_internal_bus_gripper_comm="$(arg use_internal_bus_gripper_comm)"
-      prefix="$(arg prefix)"
       use_fake_hardware="$(arg use_fake_hardware)"
-      fake_sensor_commands="$(arg fake_sensor_commands)" >
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      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)" >
+      <origin xyz="0 0 0.0" rpy="0 0 0" />
     </xacro:load_robot>
 
     <!-- Environment -->
     <xacro:cube link_name="table" connected_to="world" length="2.00" width="2.00" height="0.25" alpha="0.3">
-      <origin xyz="0 0 ${-0.25/2}" rpy="0 0 0"/>
+      <origin xyz="0 0 -0.145" rpy="0 0 0"/>
     </xacro:cube>
 
     <!-- wrist realsense -->
@@ -95,17 +107,35 @@
         <parent link="$(arg prefix)bracelet_link" />
         <child link="$(arg prefix)wrist_camera_link_visual" />
       </joint>
+    </xacro:if>
 
-      <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
-      <!-- we add this link here and make it coincident with the tool_frame but rotate it to put the camera normal to the surface -->
-      <link name="manual_grasp_link"/>
+    <!-- Visual aids for grasping -->
+    <link name="finger_tip_closed_point">
+      <visual>
+        <geometry>
+          <sphere radius="0.007"/>
+        </geometry>
+        <material name="">
+              <color rgba="0 1.0 0 0.7"/>
+        </material>
+      </visual>
+    </link>
+
+    <joint name="finger_tip_closed_point_joint" type="fixed">
+      <parent link="robotiq_85_base_link"/>
+      <child link = "finger_tip_closed_point"/>
+      <origin xyz="0 0 0.16" rpy="0 0 0"/>
+    </joint>
 
-      <joint name="manual_grasp_joint" type="fixed">
-        <parent link="$(arg prefix)tool_frame"/>
-        <child link = "manual_grasp_link"/>
-        <origin xyz="0 0 0" rpy="${3.14 / 180.0 * 10} 0 0"/>
-      </joint>
-    </xacro:if>
+    <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
+    <!-- we add this link here and make it coincident with the manual_grasp_link but rotate it to put the camera normal to the surface -->
+    <link name="manual_grasp_link"/>
+
+    <joint name="manual_grasp_joint" type="fixed">
+      <parent link="$(arg prefix)end_effector_link"/>
+      <child link = "manual_grasp_link"/>
+      <origin xyz="0 0 0.15" rpy="${3.14 / 180.0 * 10} 0 0"/>
+    </joint>
 
     <!-- Camera External -->
     <link name="external_camera_link" />

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].

src/picknik_kinova_gen3_site_config/description/picknik_kinova_gen3.xacro

@@ -1,13 +1,12 @@
 <?xml version="1.0"?>
-
 <robot name="gen3" xmlns:xacro="http://ros.org/wiki/xacro">
-
     <!-- Arguments for Kinova -->
     <xacro:arg name="arm" default="gen3" />
     <xacro:arg name="dof" default="7" />
     <xacro:arg name="vision" default="false" />
     <xacro:arg name="robot_ip" default="yyy.yyy.yyy.yyy" />
     <xacro:arg name="gripper" default="" />
+    <xacro:arg name="gripper_joint_name" default="robotiq_85_left_knuckle_joint" />
     <xacro:arg name="username" default="admin" />
     <xacro:arg name="password" default="admin" />
     <xacro:arg name="port" default="10000" />
@@ -18,6 +17,12 @@
     <xacro:arg name="prefix" default="" />
     <xacro:arg name="use_fake_hardware" default="false" />
     <xacro:arg name="fake_sensor_commands" default="false" />
+    <xacro:arg name="sim_gazebo" default="false" />
+    <xacro:arg name="sim_ignition" default="false" />
+    <xacro:arg name="sim_isaac" default="false" />
+    <xacro:arg name="use_external_cable" default="false" />
+    <xacro:arg name="gripper_max_velocity" default="100.0" />
+    <xacro:arg name="gripper_max_force" default="100.0" />
     <xacro:arg name="wrist_realsense" default="true" />
 
     <!-- Import macros for main hardware components -->
@@ -33,8 +38,10 @@
     <!-- Load the robot -->
     <xacro:load_robot
       parent="world"
+      prefix="$(arg prefix)"
       arm="$(arg arm)"
       gripper="$(arg gripper)"
+      gripper_joint_name="$(arg gripper_joint_name)"
       dof="$(arg dof)"
       vision="$(arg vision)"
       robot_ip="$(arg robot_ip)"
@@ -45,10 +52,15 @@
       session_inactivity_timeout_ms="$(arg session_inactivity_timeout_ms)"
       connection_inactivity_timeout_ms="$(arg connection_inactivity_timeout_ms)"
       use_internal_bus_gripper_comm="$(arg use_internal_bus_gripper_comm)"
-      prefix="$(arg prefix)"
       use_fake_hardware="$(arg use_fake_hardware)"
-      fake_sensor_commands="$(arg fake_sensor_commands)" >
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      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)" >
+      <origin xyz="0 0 0.0" rpy="0 0 0" />
     </xacro:load_robot>
 
     <!-- Environment -->
@@ -95,17 +107,35 @@
         <parent link="$(arg prefix)bracelet_link" />
         <child link="$(arg prefix)wrist_camera_link_visual" />
       </joint>
+    </xacro:if>
 
-      <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
-      <!-- we add this link here and make it coincident with the tool_frame but rotate it to put the camera normal to the surface -->
-      <link name="manual_grasp_link"/>
+    <!-- Visual aids for grasping -->
+    <link name="finger_tip_closed_point">
+      <visual>
+        <geometry>
+          <sphere radius="0.007"/>
+        </geometry>
+        <material name="">
+              <color rgba="0 1.0 0 0.7"/>
+        </material>
+      </visual>
+    </link>
+
+    <joint name="finger_tip_closed_point_joint" type="fixed">
+      <parent link="robotiq_85_base_link"/>
+      <child link = "finger_tip_closed_point"/>
+      <origin xyz="0 0 0.16" rpy="0 0 0"/>
+    </joint>
 
-      <joint name="manual_grasp_joint" type="fixed">
-        <parent link="$(arg prefix)tool_frame"/>
-        <child link = "manual_grasp_link"/>
-        <origin xyz="0 0 0" rpy="${3.14 / 180.0 * 10} 0 0"/>
-      </joint>
-    </xacro:if>
+    <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
+    <!-- we add this link here and make it coincident with the manual_grasp_link but rotate it to put the camera normal to the surface -->
+    <link name="manual_grasp_link"/>
+
+    <joint name="manual_grasp_joint" type="fixed">
+      <parent link="$(arg prefix)end_effector_link"/>
+      <child link = "manual_grasp_link"/>
+      <origin xyz="0 0 0.15" rpy="${3.14 / 180.0 * 10} 0 0"/>
+    </joint>
 
     <!-- Camera External -->
     <link name="external_camera_link" />