Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

kortex_arm_simulation crashes if the arm is mounted on a mobile robot #163

Open
civerachb-cpr opened this issue Jun 24, 2021 · 29 comments
Open

Comments

@civerachb-cpr
Copy link

A common use case (for us anyway) is mounting the Gen3 or Gen3 Lite onto a mobile robot platform (e.g. Clearpath's Jackal, Dingo, or Husky). Unfortunately, it appears to be impossible to run a complete simulation of the arm and mobile platform because of some issues inside kortex_arm_simulation.cpp:

  1. The robot_description path is hard-coded. When mounting the arm on the real robot, we modify the base robot's URDF to include the arm macro. For example on a Dingo with a Gen3 Lite arm we would use https://github.com/dingo-cpr/dingo_manipulation/blob/main/dingo_kinova_description/urdf/dingo_gen3_lite_description.urdf.xacro to attach the arm to the robot. This results in the robot description parameter being /robot_description, but

    model.initParam("/" + m_robot_name + "/robot_description");
    hard-codes the path to be /<arm-name>/robot_description which does not exist in this use-case, and there's no way to modify this path without needing to recompile the driver.

  2. Even if we do change the model.initParam line in kortex_arm_simulator.cpp, the fact that this class also tries to launch MoveIt and find planning groups causes the driver to crash anyway. We don't want to launch Moveit as part of the simulated driver; we just want to be able to get the joint states publishing correctly. We can launch Moveit ourselves (e.g. using https://github.com/dingo-cpr/dingo_manipulation/blob/main/dingo_gen3_lite_moveit_config/launch/move_group.launch) -- again, there doesn't appear to be any way to disable auto-launching Moveit without making some major changes to the driver and recompiling it.

Is there a different node I should be launching to launch a simulated arm driver that just launches the core driver with a configurable robot_description path and without automatically starting Moveit planning groups? The launch file I'm using to start the arm is this:

<launch>
  <!-- Arm configuration -->
  <arg name="arm" default="$(optenv DINGO_ARM_MODEL gen3_lite)"/>
  <arg name="dof" default="$(optenv DINGO_ARM_DOF 6)"/>
  <arg name="vision" default="false"/>
  <arg name="use_hard_limits" default="false"/>
  <arg name="gripper" default="$(optenv DINGO_ARM_GRIPPER gen3_lite_2f)"/>

  <!-- Namespace -->
  <arg name="robot_name" default="$(optenv DINGO_ARM_PREFIX arm)"/>
  <arg name="prefix" default="$(optenv DINGO_ARM_PREFIX arm)_"/>

  <!-- Kortex API options -->
  <arg name="ip_address" default="$(optenv DINGO_ARM_HOST 192.168.131.40)"/>
  <arg name="username" default="admin"/>
  <arg name="password" default="admin"/>
  <arg name="cyclic_data_publish_rate" default="100"/> <!--Hz-->
  <arg name="api_rpc_timeout_ms" default="2000"/> <!--milliseconds-->
  <arg name="api_session_inactivity_timeout_ms" default="35000"/> <!--milliseconds-->
  <arg name="api_connection_inactivity_timeout_ms" default="20000"/> <!--milliseconds-->
  <arg name="sim" default="false" />

  <!-- Action server params -->
  <arg name="default_goal_time_tolerance" default="0.5"/> <!--seconds-->
  <arg name="default_goal_tolerance" default="0.005"/> <!--radians-->

  <group ns="$(arg robot_name)">
      <!-- Start the kortex_driver node -->
      <node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->
          <param name="sim" value="$(arg sim)"/>
          <param name="ip_address" value="$(arg ip_address)"/>
          <param name="username" value="$(arg username)"/>
          <param name="password" value="$(arg password)"/>
          <param name="cyclic_data_publish_rate" value="$(arg cyclic_data_publish_rate)"/>
          <param name="api_rpc_timeout_ms" value="$(arg api_rpc_timeout_ms)"/>
          <param name="api_session_inactivity_timeout_ms" value="$(arg api_session_inactivity_timeout_ms)"/>
          <param name="api_connection_inactivity_timeout_ms" value="$(arg api_connection_inactivity_timeout_ms)"/>
          <param name="default_goal_time_tolerance" value="$(arg default_goal_time_tolerance)"/>
          <param name="default_goal_tolerance" value="$(arg default_goal_tolerance)"/>
          <param name="arm" value="$(arg arm)"/>
          <param name="gripper" value="$(arg gripper)"/>
          <param name="dof" value="$(arg dof)"/>
          <param name="use_hard_limits" value="$(arg use_hard_limits)"/>
          <param name="robot_name" value="$(arg robot_name)"/>
          <param name="prefix" value="$(arg prefix)"/>
          <rosparam command="load" file="$(find kortex_description)/arms/$(arg arm)/$(arg dof)dof/config/joint_limits.yaml" subst_value="true"/>
          <!-- If there is a gripper, load the active joint names for it -->
          <rosparam command="load" file="$(find kortex_description)/grippers/$(arg gripper)/config/joint_limits.yaml" unless="$(eval not arg('gripper'))" subst_value="true"/>
      </node>
  </group>

  <node name="joint_state_relay" type="relay" pkg="topic_tools" args="/$(arg robot_name)/base_feedback/joint_state /joint_states" />
</launch>

with the sim parameter set to true. This same launch file works properly on a physical robot (with the sim parameter set to false). My goal is to be able to have the simulation and real robot work as-similarly as possible. If I need to use a different node than the kortex_driver/kortex_arm_driver instead, what node should I use instead?

@felixmaisonneuve
Copy link
Contributor

Hi @civerachb-cpr,

I am not sure I understand everything in your question, so I will try to answer it the best I can.

First, you seem to have trouble getting the robot_description. In kortex_arm_simulation.cpp, the robot_description is found using the hardcoded path "/" + m_robot_name + "/robot_description". This is because in our launchfile (kortex_driver.launch) we have something like

<param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/$(arg arm).xacro dof:=$(arg dof) vision:=$(arg vision) sim:=false"

defined within the robot_name namespace (<group ns="$(arg robot_name)">). This will publish the robot_description in "/m_robot_name/robot_description".
I don't see where you publish your robot_description using your xacro file. I think you should have something like

 <group ns="$(arg robot_name)">

      <!-- Load robot description -->
      <param name="robot_description" command="$(find xacro)/xacro ...

      <!-- Start the kortex_driver node -->
      <node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->
          <param name="sim" value="$(arg sim)"/>
          <param name="ip_address" value="$(arg ip_address)"/>
...

Second, you mentionned kortex_arm_simulation.cpp is starting Moveit, which is not the case (or at least I don't see anything related to moveit in kortex_arm_simulation.cpp).
In our launch file (kortex_driver.launch) however, we do have 2 arguments (start_rviz and start_moveit) that are set to true by default. These two will start the simulation by launching ...$(arm)_moveit_config/launch/move_group.launch. So, unless you set those to false when launching the driver (e.g. roslaunch kortex_driver kortex_driver.launch start_rviz:=false start_moveit:=false) it will automatically launch the simulation.

I am not sure what you are trying to achieve with the sim argument.

@civerachb-cpr
Copy link
Author

Sorry, let me clarify:
I am using a Clearpath Dingo with a Gen3 Lite arm mounted to it. We've previously done this successfully on a physical robot, using the process outlined here: http://www.clearpathrobotics.com/assets/guides/melodic/dingo/manipulation.html, and are now looking into supporting this entirely within Gazebo using a simulated robot.

Basically, we're using the Dingo as the main robot description, and adding the arm using the URDF Extras environment variable we support. You can see the Dingo description here: https://github.com/dingo-cpr/dingo/blob/melodic-devel/dingo_description/urdf/dingo-d.urdf.xacro.

Specifically, at the end of the file (see: https://github.com/dingo-cpr/dingo/blob/06cdeee63212374608d0add4a780f49fca5d6ecc/dingo_description/urdf/dingo-d.urdf.xacro#L246) we can load a URDF stub to add additional links and joints to the base platform. In our setup, that environment variable points to the location of this file on-disk, which simply connects the Gen3 Lite xacro macro to one of the mounting points of the base platform.

The model, viewed in Rviz looks like this:

dingo-kinova-urdf

When we configure the Dingo's URDF as described above and launch the simulation the model loads correctly, but the arm is completely slack and clips through the robot model. This, I assume, is because there is no ROS node running that is publishing the joint states of the arm joints. Echoing /tf there's nothing publishing any of the arm_* joints, which is what I would expect in this situation; the base platform node for Dingo is running, but the Kinova driver is not.

dingo-kinova-gazebo-no-driver

My goal is to be able to get the kortex_driver running to control the arm inside Gazebo and have the joint states properly published & simulated.

If I launch the kortex_driver without setting the sim argument, it attempts to connect to a physical arm, which is obviously not what I want to happen in this case. My assumption is that launching the driver in simulation mode should let me control the simulated arm. But when I try launching the driver in sim mode with my simulated robot configured as-described above, the node crashes.

I don't see where you publish your robot_description using your xacro file. I think you should have something like <param name="robot_description" command="$(find xacro)/xacro ...

The robot description is provided by the dingo_description package: https://github.com/dingo-cpr/dingo/blob/melodic-devel/dingo_description/launch/description.launch

When working with a physical arm mounted to a physical robot this is sufficient, and the robot works correctly. But when I launch the kortex_driver with the sim flag set to true, I get an exception that the robot_description parameter is not found in /arm/robot_description. But on the physical robot, under the same configuration conditions, the driver does not expect that parameter.

On the real robot, if we attempt to launch the kortex_driver using the standard launch file included in the kortex_driver package, the Dingo's robot_description parameter gets overwritten by Kinova's, and the robot model winds up in a broken state. We want to use the complete robot_description parameter published by dingo_description, which appears to be possible with the non-simulated version of kortex_driver.

If I edit

model.initParam("/" + m_robot_name + "/robot_description");
to use /robot_description, which is the description parameter we use when working on the physical robot, I can successfully load the parameter, but then the node crashes when looking up the move groups used by moveit.

For reference, here's the output of rosparam dump when my Gazebo simulation is running

$ rosparam dump
arm:
  arm_driver:
    api_connection_inactivity_timeout_ms: 20000
    api_rpc_timeout_ms: 2000
    api_session_inactivity_timeout_ms: 35000
    arm: gen3_lite
    cyclic_data_publish_rate: 100
    default_goal_time_tolerance: 0.5
    default_goal_tolerance: 0.005
    dof: 6
    gripper: gen3_lite_2f
    gripper_joint_limits_max: [-0.09]
    gripper_joint_limits_min: [0.96]
    gripper_joint_names: [arm_right_finger_bottom_joint]
    ip_address: 192.168.131.40
    joint_names: [arm_joint_1, arm_joint_2, arm_joint_3, arm_joint_4, arm_joint_5,
      arm_joint_6]
    maximum_accelerations: [1.0, 0.5, 0.4, 1.0, 10.0, 10.0]
    maximum_velocities: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
    password: admin
    prefix: arm_
    robot_name: arm
    sim: true
    use_hard_limits: false
    username: admin
  is_initialized: false
bluetooth_teleop:
  joy_node: {autorepeat_rate: 20, deadzone: 0.1}
  teleop_twist_joy: {axis_angular: 0, axis_linear: 1, enable_button: 4, enable_turbo_button: 5,
    scale_angular: 1.4, scale_linear: 0.4, scale_linear_turbo: 2.0}
dingo_joint_publisher: {publish_rate: 50, type: joint_state_controller/JointStateController}
dingo_velocity_controller:
  angular:
    z: {has_acceleration_limits: true, has_velocity_limits: true, max_acceleration: 2.0,
      max_velocity: 4.0}
  cmd_vel_timeout: 0.25
  enable_odom_tf: false
  k_l: 0.1
  k_r: 0.1
  left_wheel: left_wheel
  left_wheel_radius_multiplier: 1.0
  linear:
    x: {has_acceleration_limits: true, has_velocity_limits: true, max_acceleration: 1.0,
      max_velocity: 1.3}
  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
  publish_rate: 50.0
  right_wheel: right_wheel
  right_wheel_radius_multiplier: 1.0
  twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
  type: diff_drive_controller/DiffDriveController
  wheel_radius_multiplier: 1.0
  wheel_separation_multiplier: 1.0
ekf_localization:
  base_link_frame: base_link
  frequency: 50
  imu0: /imu/data
  imu0_config: [false, false, false, false, false, false, false, false, false, false,
    false, true, false, false, false]
  imu0_differential: false
  odom0: /dingo_velocity_controller/odom
  odom0_config: [false, false, false, false, false, false, true, true, false, false,
    false, true, false, false, false]
  odom0_differential: false
  odom_frame: odom
  two_d_mode: true
  world_frame: odom
gazebo: {auto_disable_bodies: false, cfm: 0.0, contact_max_correcting_vel: 100.0,
  contact_surface_layer: 0.001, enable_ros_network: true, erp: 0.2, gravity_x: 0.0,
  gravity_y: 0.0, gravity_z: -9.8, max_contacts: 20, max_update_rate: 1000.0, sor_pgs_iters: 50,
  sor_pgs_precon_iters: 0, sor_pgs_rms_error_tol: 0.0, sor_pgs_w: 1.3, time_step: 0.001}
gazebo_ros_control:
  pid_gains:
    left_wheel: {antiwindup: false, d: 0.0001, i: 1000000.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
      p: 0.2}
    right_wheel: {antiwindup: false, d: 0.0001, i: 1000000.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
      p: 0.2}
imu:
  data:
    accel: {drift: 0.005, drift_frequency: 0.0002777777777777778, gaussian_noise: 0.005,
      offset: 0.0, scale_error: 1.0}
    rate: {drift: 0.005, drift_frequency: 0.0002777777777777778, gaussian_noise: 0.005,
      offset: 0.0, scale_error: 1.0}
    yaw: {drift: 0.0, drift_frequency: 0.0002777777777777778, gaussian_noise: 0.0,
      offset: 0.0, scale_error: 1.0}
joy_node: {dev: /dev/input/ps4}
robot_description: "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<!-- ===================================================================================\
  \ -->\n<!-- |    This document was autogenerated by xacro from /home/civerachb/Code/dingo_ws/src/dingo/dingo_description/urdf/dingo.urdf.xacro\
  \ | -->\n<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                \
  \                 | -->\n<!-- ===================================================================================\
  \ -->\n<robot name=\"dingo\">\n  <material name=\"dark_grey\">\n    <color rgba=\"\
  0.2 0.2 0.2 1.0\"/>\n  </material>\n  <material name=\"light_grey\">\n    <color\
  \ rgba=\"0.4 0.4 0.4 1.0\"/>\n  </material>\n  <material name=\"yellow\">\n    <color\
  \ rgba=\"0.8 0.8 0.0 1.0\"/>\n  </material>\n  <material name=\"black\">\n    <color\
  \ rgba=\"0.15 0.15 0.15 1.0\"/>\n  </material>\n  <link name=\"left_wheel_link\"\
  >\n    <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n\
  \        <mesh filename=\"package://dingo_description/meshes/wheel.stl\"/>\n   \
  \   </geometry>\n      <material name=\"black\"/>\n    </visual>\n    <collision>\n\
  \      <origin rpy=\"1.57079632679 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n   \
  \     <cylinder length=\"0.02032\" radius=\"0.049\"/>\n      </geometry>\n    </collision>\n\
  \    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <mass value=\"\
  0.111\"/>\n      <inertia ixx=\"0.0003234\" ixy=\"0\" ixz=\"0\" iyy=\"0.0003234\"\
  \ iyz=\"0\" izz=\"0.00049015\"/>\n    </inertial>\n  </link>\n  <gazebo reference=\"\
  left_wheel_link\">\n    <material>Gazebo/DarkGrey</material>\n    <selfCollide>false</selfCollide>\n\
  \    <mu1 value=\"0.5\"/>\n    <mu2 value=\"0.5\"/>\n    <kp value=\"10000000.0\"\
  />\n    <kd value=\"1\"/>\n    <fdir1 value=\"1 0 0\"/>\n  </gazebo>\n  <joint name=\"\
  left_wheel\" type=\"continuous\">\n    <parent link=\"chassis_link\"/>\n    <child\
  \ link=\"left_wheel_link\"/>\n    <origin rpy=\"0 0 0\" xyz=\"0.0989838 0.2261616\
  \ 0.0345\"/>\n    <axis xyz=\"0 1 0\"/>\n  </joint>\n  <transmission name=\"left_wheel_trans\"\
  >\n    <type>transmission_interface/SimpleTransmission</type>\n    <joint name=\"\
  left_wheel\">\n      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>\n\
  \    </joint>\n    <actuator name=\"left_actuator\">\n      <mechanicalReduction>1</mechanicalReduction>\n\
  \    </actuator>\n  </transmission>\n  <link name=\"right_wheel_link\">\n    <visual>\n\
  \      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"\
  package://dingo_description/meshes/wheel.stl\"/>\n      </geometry>\n      <material\
  \ name=\"black\"/>\n    </visual>\n    <collision>\n      <origin rpy=\"1.57079632679\
  \ 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <cylinder length=\"0.02032\"\
  \ radius=\"0.049\"/>\n      </geometry>\n    </collision>\n    <inertial>\n    \
  \  <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <mass value=\"0.111\"/>\n      <inertia\
  \ ixx=\"0.0003234\" ixy=\"0\" ixz=\"0\" iyy=\"0.0003234\" iyz=\"0\" izz=\"0.00049015\"\
  />\n    </inertial>\n  </link>\n  <gazebo reference=\"right_wheel_link\">\n    <material>Gazebo/DarkGrey</material>\n\
  \    <selfCollide>false</selfCollide>\n    <mu1 value=\"0.5\"/>\n    <mu2 value=\"\
  0.5\"/>\n    <kp value=\"10000000.0\"/>\n    <kd value=\"1\"/>\n    <fdir1 value=\"\
  1 0 0\"/>\n  </gazebo>\n  <joint name=\"right_wheel\" type=\"continuous\">\n   \
  \ <parent link=\"chassis_link\"/>\n    <child link=\"right_wheel_link\"/>\n    <origin\
  \ rpy=\"0 0 0\" xyz=\"0.0989838 -0.2261616 0.0345\"/>\n    <axis xyz=\"0 1 0\"/>\n\
  \  </joint>\n  <transmission name=\"right_wheel_trans\">\n    <type>transmission_interface/SimpleTransmission</type>\n\
  \    <joint name=\"right_wheel\">\n      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>\n\
  \    </joint>\n    <actuator name=\"right_actuator\">\n      <mechanicalReduction>1</mechanicalReduction>\n\
  \    </actuator>\n  </transmission>\n  <link name=\"rear_caster\">\n    <visual>\n\
  \      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <sphere\
  \ radius=\"0.01\"/>\n      </geometry>\n      <material name=\"Black\"/>\n    </visual>\n\
  \    <collision>\n      <geometry>\n        <sphere radius=\"0.01\"/>\n      </geometry>\n\
  \      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <surface>\n        <friction>\n\
  \          <ode>\n            <mu>0.0</mu>\n            <mu2>0.0</mu2>\n       \
  \     <slip1>1.0</slip1>\n            <slip2>1.0</slip2>\n          </ode>\n   \
  \     </friction>\n      </surface>\n    </collision>\n    <inertial>\n      <mass\
  \ value=\"0.1\"/>\n      <origin xyz=\"0 0 0\"/>\n      <inertia ixx=\"0.001\" ixy=\"\
  0.0\" ixz=\"0.0\" iyy=\"0.001\" iyz=\"0.0\" izz=\"0.001\"/>\n    </inertial>\n \
  \ </link>\n  <joint name=\"rear_caster_joint\" type=\"fixed\">\n    <parent link=\"\
  chassis_link\"/>\n    <child link=\"rear_caster\"/>\n    <origin rpy=\"0 0 0\" xyz=\"\
  -0.244 0 -0.00674\"/>\n  </joint>\n  <gazebo reference=\"rear_caster\">\n    <turnGravityOff>false</turnGravityOff>\n\
  \    <kp>1000000.0</kp>\n    <kd>100.0</kd>\n    <mu1>0.2</mu1>\n    <mu2>0.2</mu2>\n\
  \    <fdir1>1 0 0</fdir1>\n    <minDepth>0.00</minDepth>\n    <implicitSpringDamper>1</implicitSpringDamper>\n\
  \  </gazebo>\n  <link name=\"base_link\"/>\n  <joint name=\"base_link_joint\" type=\"\
  fixed\">\n    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n    <parent link=\"base_link\"\
  />\n    <child link=\"chassis_link\"/>\n  </joint>\n  <link name=\"chassis_link\"\
  >\n    <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n\
  \        <mesh filename=\"package://dingo_description/meshes/diff_chassis.dae\"\
  />\n      </geometry>\n    </visual>\n    <collision>\n      <origin xyz=\"0 0 0\"\
  />\n      <geometry>\n        <mesh filename=\"package://dingo_description/meshes/diff_chassis_collision.stl\"\
  />\n      </geometry>\n    </collision>\n    <inertial>\n      <!-- Center of mass\
  \ -->\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <mass value=\"10.0\"/>\n\
  \      <!-- Moments of inertia: ( chassis without wheels ) -->\n      <inertia ixx=\"\
  0.2216\" ixy=\"0\" ixz=\"0\" iyy=\"0.2458\" iyz=\"0\" izz=\"0.4474\"/>\n    </inertial>\n\
  \  </link>\n  <link name=\"imu_link\">\n    <inertial>\n      <mass value=\"0.001\"\
  />\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <inertia ixx=\"1e-09\" ixy=\"\
  0.0\" ixz=\"0.0\" iyy=\"1e-09\" iyz=\"0.0\" izz=\"1e-09\"/>\n    </inertial>\n \
  \ </link>\n  <joint name=\"imu_joint\" type=\"fixed\">\n    <parent link=\"chassis_link\"\
  />\n    <child link=\"imu_link\"/>\n  </joint>\n  <!--\n    Mounting points for\
  \ accessories in the top channel.\n    These are flush with the top of the robot\
  \ and all oriented to face forwards.\n    Dingo-D has 6 evenly-spaced 80mm mounts\n\
  \    We name the mounts (from front to back)\n      - front\n      - front b\n \
  \     - front c\n      - rear c\n      - rear b\n      - rear\n  -->\n  <link name=\"\
  front_mount\"/>\n  <link name=\"front_b_mount\"/>\n  <link name=\"front_c_mount\"\
  />\n  <link name=\"rear_mount\"/>\n  <link name=\"rear_b_mount\"/>\n  <link name=\"\
  rear_c_mount\"/>\n  <joint name=\"front_c_mount_joint\" type=\"fixed\">\n    <origin\
  \ rpy=\"0 0 0\" xyz=\"0.040 0 0.064263\"/>\n    <parent link=\"chassis_link\"/>\n\
  \    <child link=\"front_c_mount\"/>\n  </joint>\n  <joint name=\"front_b_mount_joint\"\
  \ type=\"fixed\">\n    <origin rpy=\"0 0 0\" xyz=\"0.080 0 0\"/>\n    <parent link=\"\
  front_c_mount\"/>\n    <child link=\"front_b_mount\"/>\n  </joint>\n  <joint name=\"\
  front_mount_joint\" type=\"fixed\">\n    <origin rpy=\"0 0 0\" xyz=\"0.080 0 0\"\
  />\n    <parent link=\"front_b_mount\"/>\n    <child link=\"front_mount\"/>\n  </joint>\n\
  \  <joint name=\"rear_c_mount_joint\" type=\"fixed\">\n    <origin rpy=\"0 0 0\"\
  \ xyz=\"-0.040 0 0.064263\"/>\n    <parent link=\"chassis_link\"/>\n    <child link=\"\
  rear_c_mount\"/>\n  </joint>\n  <joint name=\"rear_b_mount_joint\" type=\"fixed\"\
  >\n    <origin rpy=\"0 0 0\" xyz=\"-0.080 0 0\"/>\n    <parent link=\"rear_c_mount\"\
  />\n    <child link=\"rear_b_mount\"/>\n  </joint>\n  <joint name=\"rear_moint_joint\"\
  \ type=\"fixed\">\n    <origin rpy=\"0 0 0\" xyz=\"-0.080 0 0\"/>\n    <parent link=\"\
  rear_b_mount\"/>\n    <child link=\"rear_mount\"/>\n  </joint>\n  <!--\n    Mounting\
  \ point for accessories on the front bumper.\n    Use with caution, but thin sensors\
  \ (e.g Raspberry Pi NoIR camera) could\n    be mounted here for low-budget, front-facing\
  \ capabilities\n  -->\n  <link name=\"front_bumper_mount\"/>\n  <joint name=\"front_bumper_mount_joint\"\
  \ type=\"fixed\">\n    <origin rpy=\"0 0 0\" xyz=\"0.274 0 0.0345125\"/>\n    <parent\
  \ link=\"chassis_link\"/>\n    <child link=\"front_bumper_mount\"/>\n  </joint>\n\
  \  <gazebo>\n    <plugin filename=\"libgazebo_ros_control.so\" name=\"gazebo_ros_control\"\
  >\n      <robotNamespace>/</robotNamespace>\n    </plugin>\n  </gazebo>\n  <gazebo>\n\
  \    <plugin filename=\"libhector_gazebo_ros_imu.so\" name=\"imu_controller\">\n\
  \      <robotNamespace>/</robotNamespace>\n      <updateRate>50.0</updateRate>\n\
  \      <bodyName>imu_link</bodyName>\n      <topicName>imu/data</topicName>\n  \
  \    <accelDrift>0.005 0.005 0.005</accelDrift>\n      <accelGaussianNoise>0.005\
  \ 0.005 0.005</accelGaussianNoise>\n      <rateDrift>0.005 0.005 0.005 </rateDrift>\n\
  \      <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>\n      <headingDrift>0.005</headingDrift>\n\
  \      <headingGaussianNoise>0.005</headingGaussianNoise>\n    </plugin>\n  </gazebo>\n\
  \  <gazebo reference=\"base_link\">\n    <turnGravityOff>false</turnGravityOff>\n\
  \  </gazebo>\n  <gazebo reference=\"chassis_link\">\n    <turnGravityOff>false</turnGravityOff>\n\
  \  </gazebo>\n  <gazebo reference=\"imu_link\">\n    <turnGravityOff>false</turnGravityOff>\n\
  \  </gazebo>\n  <!--\n    As you add to this URDF, please be aware that both the\
  \ robot and\n    simulation include it. You must retain compatibility with all of\n\
  \    the following launch files:\n\n    dingo_viz/launch/view_model.launch\n   \
  \ dingo_gazebo/launch/dingo_world.launch\n    dingo_base/launch/base.launch\n  -->\n\
  \  <!-- Run the macros -->\n  <link name=\"arm_base_link\">\n    <inertial>\n  \
  \    <origin rpy=\"0 0 0\" xyz=\"0.00244324 0.00015573 0.08616742\"/>\n      <mass\
  \ value=\"1.14608471\"/>\n      <inertia ixx=\"0.00335854\" ixy=\"3.9E-07\" ixz=\"\
  0.00010989\" iyy=\"0.003311\" iyz=\"1.91E-06\" izz=\"0.00077158\"/>\n    </inertial>\n\
  \    <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n \
  \       <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/base_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/base_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_base_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <link name=\"arm_shoulder_link\"\
  >\n    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"2.477E-05 0.02213531 0.09937686\"\
  />\n      <mass value=\"0.95974404\"/>\n      <inertia ixx=\"0.00165947\" ixy=\"\
  2E-08\" ixz=\"3.6E-07\" iyy=\"0.00140355\" iyz=\"0.00034927\" izz=\"0.00089493\"\
  />\n    </inertial>\n    <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n\
  \      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/shoulder_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/shoulder_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_shoulder_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <joint name=\"arm_joint_1\"\
  \ type=\"revolute\">\n    <origin rpy=\"0 0 0\" xyz=\"0 0 0.12825\"/>\n    <parent\
  \ link=\"arm_base_link\"/>\n    <child link=\"arm_shoulder_link\"/>\n    <axis xyz=\"\
  0 0 1\"/>\n    <limit effort=\"10\" lower=\"-2.69\" upper=\"2.69\" velocity=\"1.6\"\
  />\n  </joint>\n  <link name=\"arm_arm_link\">\n    <inertial>\n      <origin rpy=\"\
  0 0 0\" xyz=\"0.02998299 0.21154808 0.0453031\"/>\n      <mass value=\"1.17756164\"\
  />\n      <inertia ixx=\"0.01149277\" ixy=\"1E-06\" ixz=\"1.6E-07\" iyy=\"0.00102851\"\
  \ iyz=\"0.00140765\" izz=\"0.01133492\"/>\n    </inertial>\n    <visual>\n     \
  \ <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"\
  package://kortex_description/arms/gen3_lite/6dof/meshes/arm_link.STL\"/>\n     \
  \ </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824 0.820\
  \ 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin rpy=\"\
  0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/arm_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_arm_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <joint name=\"arm_joint_2\"\
  \ type=\"revolute\">\n    <origin rpy=\"1.5708 0 0\" xyz=\"0 -0.03 0.115\"/>\n \
  \   <parent link=\"arm_shoulder_link\"/>\n    <child link=\"arm_arm_link\"/>\n \
  \   <axis xyz=\"0 0 1\"/>\n    <limit effort=\"14\" lower=\"-2.69\" upper=\"2.69\"\
  \ velocity=\"1.6\"/>\n  </joint>\n  <link name=\"arm_forearm_link\">\n    <inertial>\n\
  \      <origin rpy=\"0 0 0\" xyz=\"0.0301559 0.09502206 0.0073555\"/>\n      <mass\
  \ value=\"0.59767669\"/>\n      <inertia ixx=\"0.00163256\" ixy=\"7.11E-06\" ixz=\"\
  1.54E-06\" iyy=\"0.00029798\" iyz=\"9.587E-05\" izz=\"0.00169091\"/>\n    </inertial>\n\
  \    <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n \
  \       <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/forearm_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/forearm_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_forearm_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <joint name=\"arm_joint_3\"\
  \ type=\"revolute\">\n    <origin rpy=\"-3.1416 0 0\" xyz=\"0 0.28 0\"/>\n    <parent\
  \ link=\"arm_arm_link\"/>\n    <child link=\"arm_forearm_link\"/>\n    <axis xyz=\"\
  0 0 1\"/>\n    <limit effort=\"10\" lower=\"-2.69\" upper=\"2.69\" velocity=\"1.6\"\
  />\n  </joint>\n  <link name=\"arm_lower_wrist_link\">\n    <inertial>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0.00575149 0.01000443 0.08719207\"/>\n      <mass value=\"\
  0.52693412\"/>\n      <inertia ixx=\"0.00069098\" ixy=\"2.4E-07\" ixz=\"0.00016483\"\
  \ iyy=\"0.00078519\" iyz=\"7.4E-07\" izz=\"0.00034115\"/>\n    </inertial>\n   \
  \ <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n    \
  \    <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/lower_wrist_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/lower_wrist_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_lower_wrist_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <joint name=\"arm_joint_4\"\
  \ type=\"revolute\">\n    <origin rpy=\"1.5708 0 0\" xyz=\"0 -0.14 0.02\"/>\n  \
  \  <parent link=\"arm_forearm_link\"/>\n    <child link=\"arm_lower_wrist_link\"\
  />\n    <axis xyz=\"0 0 1\"/>\n    <limit effort=\"7\" lower=\"-2.59\" upper=\"\
  2.59\" velocity=\"1.6\"/>\n  </joint>\n  <link name=\"arm_upper_wrist_link\">\n\
  \    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"0.08056517 0.00980409 0.01872799\"\
  />\n      <mass value=\"0.58097325\"/>\n      <inertia ixx=\"0.00021268\" ixy=\"\
  5.21E-06\" ixz=\"2.91E-06\" iyy=\"0.00106371\" iyz=\"1.1E-07\" izz=\"0.00108465\"\
  />\n    </inertial>\n    <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n\
  \      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/upper_wrist_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/arms/gen3_lite/6dof/meshes/upper_wrist_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_upper_wrist_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <joint name=\"arm_joint_5\"\
  \ type=\"revolute\">\n    <origin rpy=\"0 1.5708 0\" xyz=\"0.0285 0 0.105\"/>\n\
  \    <parent link=\"arm_lower_wrist_link\"/>\n    <child link=\"arm_upper_wrist_link\"\
  />\n    <axis xyz=\"0 0 1\"/>\n    <limit effort=\"7\" lower=\"-2.57\" upper=\"\
  2.57\" velocity=\"1.6\"/>\n  </joint>\n  <link name=\"arm_end_effector_link\"/>\n\
  \  <joint name=\"arm_joint_6\" type=\"revolute\">\n    <origin rpy=\"0 -1.5708 0\"\
  \ xyz=\"-0.105 0 0.0285\"/>\n    <parent link=\"arm_upper_wrist_link\"/>\n    <child\
  \ link=\"arm_end_effector_link\"/>\n    <axis xyz=\"0 0 1\"/>\n    <limit effort=\"\
  7\" lower=\"-2.59\" upper=\"2.59\" velocity=\"3.2\"/>\n  </joint>\n  <link name=\"\
  arm_dummy_link\"/>\n  <joint name=\"arm_end_effector\" type=\"fixed\">\n    <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n    <parent link=\"arm_end_effector_link\"/>\n\
  \    <child link=\"arm_dummy_link\"/>\n    <axis xyz=\"0 0 1\"/>\n    <limit effort=\"\
  0\" lower=\"0\" upper=\"0\" velocity=\"0\"/>\n  </joint>\n  <!-- Tool frame used\
  \ by the arm -->\n  <link name=\"arm_tool_frame\"/>\n  <joint name=\"arm_tool_frame_joint\"\
  \ type=\"fixed\">\n    <origin rpy=\"0 0 1.57079632679\" xyz=\"0 0 0.130\"/>\n \
  \   <parent link=\"arm_dummy_link\"/>\n    <child link=\"arm_tool_frame\"/>\n  \
  \  <axis xyz=\"0 0 0\"/>\n  </joint>\n  <!-- Gripper description -->\n  <joint name=\"\
  arm_gripper_base_joint\" type=\"fixed\">\n    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"\
  />\n    <parent link=\"arm_dummy_link\"/>\n    <child link=\"arm_gripper_base_link\"\
  />\n    <axis xyz=\"0 0 0\"/>\n  </joint>\n  <link name=\"arm_gripper_base_link\"\
  >\n    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"4.03E-06 1.08E-05 0.041397\"\
  />\n      <mass value=\"0.1395\"/>\n      <inertia ixx=\"0.00011614\" ixy=\"1E-08\"\
  \ ixz=\"4E-08\" iyy=\"0.00010327\" iyz=\"0\" izz=\"9.707E-05\"/>\n    </inertial>\n\
  \    <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n \
  \       <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/gripper_base_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/gripper_base_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_gripper_base_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <link name=\"arm_right_finger_prox_link\"\
  >\n    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"0.020257 0.0063483 6.991E-05\"\
  />\n      <mass value=\"0.018385\"/>\n      <inertia ixx=\"2.29E-06\" ixy=\"1.43E-06\"\
  \ ixz=\"0\" iyy=\"4.56E-06\" iyz=\"0\" izz=\"4.77E-06\"/>\n    </inertial>\n   \
  \ <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n    \
  \    <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/right_finger_prox_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/right_finger_prox_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_right_finger_prox_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <joint name=\"arm_right_finger_bottom_joint\"\
  \ type=\"revolute\">\n    <origin rpy=\"0 1.5708 0\" xyz=\"0 -0.030501 0.070003\"\
  />\n    <parent link=\"arm_gripper_base_link\"/>\n    <child link=\"arm_right_finger_prox_link\"\
  />\n    <axis xyz=\"0 0 1\"/>\n    <limit effort=\"1000\" lower=\"-0.09\" upper=\"\
  0.96\" velocity=\"0.6\"/>\n  </joint>\n  <link name=\"arm_right_finger_dist_link\"\
  >\n    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"0.018488 0.0011091 0\"/>\n\
  \      <mass value=\"0.010748\"/>\n      <inertia ixx=\"6.3E-07\" ixy=\"3.2E-07\"\
  \ ixz=\"0\" iyy=\"1.83E-06\" iyz=\"0\" izz=\"1.61E-06\"/>\n    </inertial>\n   \
  \ <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n    \
  \    <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/right_finger_dist_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0 0.055\
  \ 0.525 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/right_finger_dist_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_right_finger_dist_link\"\
  >\n    <material>Kortex/Blue</material>\n  </gazebo>\n  <joint name=\"arm_right_finger_tip_joint\"\
  \ type=\"revolute\">\n    <origin rpy=\"0 0 0\" xyz=\"-0.045636 0.020423 0\"/>\n\
  \    <parent link=\"arm_right_finger_prox_link\"/>\n    <child link=\"arm_right_finger_dist_link\"\
  />\n    <axis xyz=\"0 0 1\"/>\n    <limit effort=\"1000\" lower=\"-0.50\" upper=\"\
  0.21\" velocity=\"1000\"/>\n    <!--limit lower=\"-1.03\" upper=\"0.21\" effort=\"\
  1000\" velocity=\"1000\" /-->\n    <mimic joint=\"arm_right_finger_bottom_joint\"\
  \ multiplier=\"-0.676\" offset=\"0.149\"/>\n  </joint>\n  <link name=\"arm_left_finger_prox_link\"\
  >\n    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"0.020257 0.0063483 6.99E-05\"\
  />\n      <mass value=\"0.018385\"/>\n      <inertia ixx=\"2.29E-06\" ixy=\"1.43E-06\"\
  \ ixz=\"0\" iyy=\"4.56E-06\" iyz=\"0\" izz=\"4.77E-06\"/>\n    </inertial>\n   \
  \ <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n    \
  \    <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/left_finger_prox_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0.803 0.824\
  \ 0.820 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/left_finger_prox_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_left_finger_prox_link\"\
  >\n    <material>Kortex/Gray</material>\n  </gazebo>\n  <joint name=\"arm_left_finger_bottom_joint\"\
  \ type=\"revolute\">\n    <origin rpy=\"0 1.5708 0\" xyz=\"0 0.0305 0.070003\"/>\n\
  \    <parent link=\"arm_gripper_base_link\"/>\n    <child link=\"arm_left_finger_prox_link\"\
  />\n    <axis xyz=\"0 0 1\"/>\n    <limit effort=\"1000\" lower=\"-0.96\" upper=\"\
  0.09\" velocity=\"1000\"/>\n    <mimic joint=\"arm_right_finger_bottom_joint\" multiplier=\"\
  -1.0\" offset=\"0.0\"/>\n  </joint>\n  <link name=\"arm_left_finger_dist_link\"\
  >\n    <inertial>\n      <origin rpy=\"0 0 0\" xyz=\"-0.018488 0.0011091 0\"/>\n\
  \      <mass value=\"0.010748\"/>\n      <inertia ixx=\"6.3E-07\" ixy=\"3.2E-07\"\
  \ ixz=\"0\" iyy=\"1.83E-06\" iyz=\"0\" izz=\"1.61E-06\"/>\n    </inertial>\n   \
  \ <visual>\n      <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n    \
  \    <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/left_finger_dist_link.STL\"\
  />\n      </geometry>\n      <material name=\"\">\n        <color rgba=\"0 0.055\
  \ 0.525 1\"/>\n      </material>\n    </visual>\n    <collision>\n      <origin\
  \ rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n      <geometry>\n        <mesh filename=\"package://kortex_description/grippers/gen3_lite_2f/meshes/left_finger_dist_link.STL\"\
  />\n      </geometry>\n    </collision>\n  </link>\n  <gazebo reference=\"arm_left_finger_dist_link\"\
  >\n    <material>Kortex/Blue</material>\n  </gazebo>\n  <joint name=\"arm_left_finger_tip_joint\"\
  \ type=\"revolute\">\n    <origin rpy=\"0 0 0\" xyz=\"-0.045636 -0.020423 6.9901E-05\"\
  />\n    <parent link=\"arm_left_finger_prox_link\"/>\n    <child link=\"arm_left_finger_dist_link\"\
  />\n    <axis xyz=\"0 0 -1\"/>\n    <limit effort=\"1000\" lower=\"-0.50\" upper=\"\
  0.21\" velocity=\"1000\"/>\n    <!--limit lower=\"-1.03\" upper=\"0.21\" effort=\"\
  1000\" velocity=\"1000\" /-->\n    <mimic joint=\"arm_right_finger_bottom_joint\"\
  \ multiplier=\"-0.676\" offset=\"0.149\"/>\n  </joint>\n  <joint name=\"arm_mount_joint\"\
  \ type=\"fixed\">\n    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 0.0\"/>\n    <parent\
  \ link=\"front_b_mount\"/>\n    <child link=\"arm_base_link\"/>\n  </joint>\n</robot>\n\
  \n"
rosdistro: 'melodic

  '
roslaunch:
  uris: {host_cpr01954l__37263: 'http://CPR01954L:37263/', host_cpr01954l__38541: 'http://CPR01954L:38541/',
    host_cpr01954l__46773: 'http://CPR01954L:46773/'}
rosversion: '1.14.11

  '
run_id: 4865c338-d5c0-11eb-8136-70886b8f17a5
twist_marker_server: {linear_scale: 1, max_negative_linear_velocity: -1, max_positive_linear_velocity: 1}
twist_mux:
  locks:
  - {name: e_stop, priority: 255, timeout: 0.0, topic: e_stop}
  topics:
  - {name: joy, priority: 10, timeout: 0.5, topic: joy_teleop/cmd_vel}
  - {name: bt_joy, priority: 9, timeout: 0.5, topic: bluetooth_teleop/cmd_vel}
  - {name: interactive_marker, priority: 8, timeout: 0.5, topic: twist_marker_server/cmd_vel}
  - {name: external, priority: 1, timeout: 0.5, topic: cmd_vel}
use_gui: true
use_sim_time: true

Second, you mentionned kortex_arm_simulation.cpp is starting Moveit, which is not the case (or at least I don't see anything related to moveit in kortex_arm_simulation.cpp).

Right here there appears to be a Moveit client starting, which is directly linked to the crash I'm seeing:

m_moveit_arm_interface.reset(new moveit::planning_interface::MoveGroupInterface(ARM_PLANNING_GROUP));

When I launch with the fixed robot_description parameter path, I get the following error:

[FATAL] [1624631972.081061706, 2431.902000000]: Group 'arm' was not found.
terminate called after throwing an instance of 'std::runtime_error'
  what():  Group 'arm' was not found.

The only reference to a group named arm I was able to find in the code was

static const std::string ARM_PLANNING_GROUP = "arm";
, which, along with the gripper planning group name on the next line, appear to be part of the Moveit client that the simulator is launching.

At this point I'm not sure what my next logical step is. I'd rather not go through kortex_arm_simulation.cpp and manually remove all of the Moveit client code and recompile. But is that the correct course of action? Or am I missing something and there's a different/better launch file I should be using to launch the Kortex driver simulation while preserving the Dingo URDF and robot_description parameter?

@felixmaisonneuve
Copy link
Contributor

Ok, I understand your problem, and I will try my best to help you.

When you start the driver with sim:=true, it launches kortex_arm_simulation.cpp, which tries to read the robot description from the ros param /arm/robot_description and crashes because it is not there. This does not happen with a real arm because it communicates with the actual robot and verify the configuration from there.

If you change model.initParam("/" + m_robot_name + "/robot_description"); to model.initParam("/robot_description"); it crashes when trying to create the default actions in moveit. I think this is not essential and you could simply try to comment the lines below (involving moveit) in kortex_arm_simulation.cpp (this includes line 149):

// Start MoveIt client
m_moveit_arm_interface.reset(new moveit::planning_interface::MoveGroupInterface(ARM_PLANNING_GROUP));
if (IsGripperPresent())
{
m_moveit_gripper_interface.reset(new moveit::planning_interface::MoveGroupInterface(GRIPPER_PLANNING_GROUP));
}
// Create default actions
CreateDefaultActions();

This should allow you to finsh to initialization of the kortex_arm_driver.cpp.
If it does, you should see is_initialized: true instead of is_initialized: false in your rosparam dump

@civerachb-cpr
Copy link
Author

Ok, thanks. I'll try that out and let you know how it goes.

Assuming it does work I may submit a PR to add some additional ROS params to make this easier to configure without needing to recompile the driver.

@civerachb-cpr
Copy link
Author

I made the changes you suggested, and it seems like I'm making progress in the right direction. However, I'm noticing more inconsistencies between the real arm driver and the simulated one that seem to be causing further issues.

After starting the kortex_driver with the simulation flag set to true the arm remains slack in Gazebo. It appears nothing is actually publishing the state of the arm joints.

When using the real arm, the joint states are published on /${robot_name}/base_feedback/joint_state, but no such topic seems to exist when I run in simulation. This has a trickle-down effect, as normally I would use a topic_tools/relay to republish the base_feedback/joint_state data to /joint_states, which is subscribed to by the main robot_state_publisher.

The only topics published by the arm driver when I run in simulation are:

Node [/arm/arm_driver]
Publications: 
 * /arm/action_topic [kortex_driver/ActionNotification]
 * /arm/arm_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/cancel [actionlib_msgs/GoalID]
 * /arm/arm_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/goal [control_msgs/FollowJointTrajectoryActionGoal]
 * /arm/arm_joint_1_position_controller/command [std_msgs/Float64]
 * /arm/arm_joint_2_position_controller/command [std_msgs/Float64]
 * /arm/arm_joint_3_position_controller/command [std_msgs/Float64]
 * /arm/arm_joint_4_position_controller/command [std_msgs/Float64]
 * /arm/arm_joint_5_position_controller/command [std_msgs/Float64]
 * /arm/arm_joint_6_position_controller/command [std_msgs/Float64]
 * /arm/arm_state_topic [kortex_driver/ArmStateNotification]
 * /arm/configuration_change_topic [kortex_driver/ConfigurationChangeNotification]
 * /arm/control_configuration_topic [kortex_driver/ControlConfigurationNotification]
 * /arm/control_mode_topic [kortex_driver/ControlModeNotification]
 * /arm/controller_topic [kortex_driver/ControllerNotification]
 * /arm/factory_topic [kortex_driver/FactoryNotification]
 * /arm/kortex_error [kortex_driver/KortexError]
 * /arm/mapping_info_topic [kortex_driver/MappingInfoNotification]
 * /arm/network_topic [kortex_driver/NetworkNotification]
 * /arm/operating_mode_topic [kortex_driver/OperatingModeNotification]
 * /arm/protection_zone_topic [kortex_driver/ProtectionZoneNotification]
 * /arm/robot_event_topic [kortex_driver/RobotEventNotification]
 * /arm/safety_topic [kortex_driver/SafetyNotification]
 * /arm/sequence_info_topic [kortex_driver/SequenceInfoNotification]
 * /arm/servoing_mode_topic [kortex_driver/ServoingModeNotification]
 * /arm/user_topic [kortex_driver/UserNotification]
 * /rosout [rosgraph_msgs/Log]

What topic should the joint states of the simulated arm be published on?

civerachb-cpr added a commit to civerachb-cpr/ros_kortex that referenced this issue Jul 15, 2021
@felixmaisonneuve
Copy link
Contributor

Hi @civerachb-cpr,

I noticed you are using a prefix when launching the driver. I think this might be the problem here.
I was able to get similar output to yours while using a prefix.

I recently (last week) realised that something is not working when using a prefix and I haven't looked into it yet. I noticed this while looking at this PR.
This is a major inconvenience and I don't know if it was working at some point (probably) and broke or if it never worked in the first place. This is probably something I will look into soon

If it is possible, you might try to not use any prefix.
In my case, I am able to get the complete list of topics when I am not using any prefix, but, when I use the prefix argument, I get basically the same topics for the driver node and the simulation is not working.

Hopefully it is possible for you to use prefix="" in your launch file and it will work.
Sorry for the inconveniance,
Felix

@felixmaisonneuve
Copy link
Contributor

After further inspection, I don't think it is related to the prefix argument.
I tried again and /arm/base_feedback and /arm/base_feedback/joint_state are published correctly, so I must have missed them last week.

Those two topics are supposed to be published at the end of the driver initialization

m_pub_base_feedback = m_node_handle.advertise<kortex_driver::BaseCyclic_Feedback>("base_feedback", 1000);
m_pub_joint_state = m_node_handle.advertise<sensor_msgs::JointState>("base_feedback/joint_state", 1000);
if (m_is_real_robot)
{
m_publish_feedback_thread = std::thread(&KortexArmDriver::publishRobotFeedback, this);
}
else
{
m_publish_feedback_thread = std::thread(&KortexArmDriver::publishSimulationFeedback, this);
}

Can you make sure everything has been initialized correctly with the param is_initialized that should be true (for example by doing a rosparam dump).

If everything looks good, this might indicate the problem is coming form GetFeedback() :

m_pub_base_feedback.publish(m_simulator->GetFeedback());

Also, looking at your output, I get 2 topics related to the gripper that you do not have : /arm/arm_gen3_lite_2f_gripper_controller/gripper_cmd/cancel and /arm/arm_gen3_lite_2f_gripper_controller/gripper_cmd/goal.
Can you make sure you set gripper:=gen3_lite_2f correctly in your launch file?

Finally, I see that one of your topic is /arm/control_mode_topic [kortex_driver/ControlModeNotification]. ControlModeNotification has been renamed to Base_ControlModeNotification in Kortex release 2.3.
I do not think it will fix your problem, but it might be a good idea to update your repo if it not too much trouble

@felixmaisonneuve
Copy link
Contributor

Hi @civerachb-cpr,

Do you have any update on this?
I will wait for a couple of weeks and close this issue if I don't get an answer.

Regards,
Felix

@civerachb-cpr
Copy link
Author

Hi, sorry for the delay replying to this. I'll get straight into answering your questions:

1- Yes, the gripper parameter is set to gen3_lite_2f
2- I tried setting prefix="" in the launch file, but that didn't help; when I tried launching the arm driver I got the following error:

...
[ INFO] [1629726247.428386962, 1654.489000000]: -------------------------------------------------
[ INFO] [1629726247.437425354, 1654.496000000]: Simulating arm with following characteristics:
[ INFO] [1629726247.437461693, 1654.496000000]: Arm type : gen3_lite
[ INFO] [1629726247.437475851, 1654.496000000]: Gripper type : gen3_lite_2f
[ INFO] [1629726247.437487258, 1654.496000000]: Arm namespace : kinova_arm
[ INFO] [1629726247.437503305, 1654.496000000]: URDF prefix : 
[ERROR] [1629726247.441029520, 1654.498000000]: Failed to extract kinematic chain from parsed tree!
terminate called after throwing an instance of 'std::runtime_error'
  what():  Failed to extract kinematic chain from parsed tree!

I suspect this may be due to a mismatch in prefixes between the URDF and the driver launch file? In order to keep the joint names unique across the robot we do use the prefix parameter of the load_robot URDF macro.

Launching with the prefix argument set normally appears to run without any errors, but is_initialized is still false in my rosparam dump output:

Roslaunch output:

SUMMARY
========

PARAMETERS
 * /kinova_arm/kinova_arm_driver/api_connection_inactivity_timeout_ms: 20000
 * /kinova_arm/kinova_arm_driver/api_rpc_timeout_ms: 2000
 * /kinova_arm/kinova_arm_driver/api_session_inactivity_timeout_ms: 35000
 * /kinova_arm/kinova_arm_driver/arm: gen3_lite
 * /kinova_arm/kinova_arm_driver/cyclic_data_publish_rate: 40
 * /kinova_arm/kinova_arm_driver/default_goal_time_tolerance: 0.5
 * /kinova_arm/kinova_arm_driver/default_goal_tolerance: 0.005
 * /kinova_arm/kinova_arm_driver/dof: 6
 * /kinova_arm/kinova_arm_driver/gripper: gen3_lite_2f
 * /kinova_arm/kinova_arm_driver/gripper_joint_limits_max: [-0.09]
 * /kinova_arm/kinova_arm_driver/gripper_joint_limits_min: [0.96]
 * /kinova_arm/kinova_arm_driver/gripper_joint_names: ['kinova_arm_righ...
 * /kinova_arm/kinova_arm_driver/ip_address: 192.168.1.10
 * /kinova_arm/kinova_arm_driver/joint_names: ['kinova_arm_join...
 * /kinova_arm/kinova_arm_driver/maximum_accelerations: [1.0, 0.5, 0.4, 1...
 * /kinova_arm/kinova_arm_driver/maximum_velocities: [0.5, 0.5, 0.5, 0...
 * /kinova_arm/kinova_arm_driver/password: admin
 * /kinova_arm/kinova_arm_driver/prefix: kinova_arm_
 * /kinova_arm/kinova_arm_driver/robot_name: kinova_arm
 * /kinova_arm/kinova_arm_driver/sim: True
 * /kinova_arm/kinova_arm_driver/use_hard_limits: False
 * /kinova_arm/kinova_arm_driver/username: admin
 * /rosdistro: melodic
 * /rosversion: 1.14.11

NODES
  /kinova_arm/
    joint_state_relay (topic_tools/relay)
    kinova_arm_driver (kortex_driver/kortex_arm_driver)

ROS_MASTER_URI=http://localhost:11311

process[kinova_arm/kinova_arm_driver-1]: started with pid [3776]
process[kinova_arm/joint_state_relay-2]: started with pid [3777]
[ INFO] [1629726259.594322042]: -------------------------------------------------
[ INFO] [1629726259.594951547]: Initializing Kortex Driver's services...
[ INFO] [1629726261.745428358, 1665.215000000]: Kortex Driver's services initialized correctly.
[ INFO] [1629726261.745473183, 1665.215000000]: -------------------------------------------------
[ INFO] [1629726261.754778920, 1665.223000000]: Simulating arm with following characteristics:
[ INFO] [1629726261.754809371, 1665.223000000]: Arm type : gen3_lite
[ INFO] [1629726261.754824325, 1665.223000000]: Gripper type : gen3_lite_2f
[ INFO] [1629726261.754836299, 1665.223000000]: Arm namespace : kinova_arm
[ INFO] [1629726261.754848030, 1665.223000000]: URDF prefix : kinova_arm_
$ rosparam dump|grep is_initialized
  is_initialized: false

I do not see anything published on the base_feedback/joint_state nor base_feedback topics, likely because the initialization is failing.

I just pulled the latest melodic-devel branch this morning and tested again; same results. Is there a different branch I should try that hasn't been merged yet?

@felixmaisonneuve
Copy link
Contributor

Hi @civerachb-cpr,

Did you delete some of the rows in your roslaunch output? I get way more stuff than you. I was wondering if you did it on purpose to make more readable.

Also, your simulation driver does not seem to initialize correctly and I am not sure why.
Do you get any error message in your roslaunch output?

Can you also give the output of rostopic list. I see the driver is looking for topic /<robot_name>/joint_states :

m_sub_joint_state = m_node_handle.subscribe("/" + m_robot_name + "/" + "joint_states", 1, &KortexArmSimulation::cb_joint_states, this);

If it is not there, I think it might explain why all your topics are empty.

To answer your question, there is no other branch you should try it on.

Regards,
Felix

@yj-Tang
Copy link

yj-Tang commented Dec 22, 2021

@civerachb-cpr Hi, have you solved this problem? I am integrating Husky and Kinova Gen3 Lite in Gazebo, and have the same problem. Could you give any updates on how you solved them?

@felixmaisonneuve
Copy link
Contributor

Hi @yj-Tang,

I don't know if @civerachb-cpr managed to fix the issue, but if you have the same issue, I can try to help you.

What commit of the repo are you using?
Also, what is the full output of your roslaunch and rostopic list commands?

Best,
Felix

@yj-Tang
Copy link

yj-Tang commented Jan 3, 2022

Hi @felixmaisonneuve,
Thanks for the reply! I somehow have the kortex arm and the mobile robot simulated together. But the kortex driver is always died after I launched the simulation (kinova_arm_driver-8] process has died [pid 1502665, exit code -11), which confused me. The output of the roslaunch is shown below:

.. logging to /home/yujie/.ros/log/be55b2e2-6ca6-11ec-9974-4d8c6a873cd5/roslaunch-ricks-Alienware-m15-R6-1502516.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: in-order processing became default in ROS Melodic. You can drop the option.
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
started roslaunch server http://ricks-Alienware-m15-R6:38451/

SUMMARY
========

PARAMETERS
 * /ekf_localization/base_link_frame: base_link
 * /ekf_localization/frequency: 50
 * /ekf_localization/imu0: imu/data
 * /ekf_localization/imu0_config: [False, False, Fa...
 * /ekf_localization/imu0_differential: True
 * /ekf_localization/imu0_queue_size: 10
 * /ekf_localization/imu0_remove_gravitational_acceleration: True
 * /ekf_localization/odom0: husky_velocity_co...
 * /ekf_localization/odom0_config: [False, False, Fa...
 * /ekf_localization/odom0_differential: False
 * /ekf_localization/odom0_queue_size: 10
 * /ekf_localization/odom_frame: odom
 * /ekf_localization/two_d_mode: True
 * /ekf_localization/world_frame: odom
 * /gazebo/enable_ros_network: True
 * /gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/d: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/i: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/p: 10.0
 * /gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/d: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/i: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/p: 1.0
 * /gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/d: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/i: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/p: 10.0
 * /gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/d: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/i: 0.0
 * /gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/p: 1.0
 * /husky_joint_publisher/publish_rate: 50
 * /husky_joint_publisher/type: joint_state_contr...
 * /husky_velocity_controller/angular/z/has_acceleration_limits: True
 * /husky_velocity_controller/angular/z/has_velocity_limits: True
 * /husky_velocity_controller/angular/z/max_acceleration: 6.0
 * /husky_velocity_controller/angular/z/max_velocity: 2.0
 * /husky_velocity_controller/base_frame_id: base_link
 * /husky_velocity_controller/cmd_vel_timeout: 0.25
 * /husky_velocity_controller/enable_odom_tf: False
 * /husky_velocity_controller/estimate_velocity_from_position: False
 * /husky_velocity_controller/left_wheel: ['front_left_whee...
 * /husky_velocity_controller/linear/x/has_acceleration_limits: True
 * /husky_velocity_controller/linear/x/has_velocity_limits: True
 * /husky_velocity_controller/linear/x/max_acceleration: 3.0
 * /husky_velocity_controller/linear/x/max_velocity: 1.0
 * /husky_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/publish_rate: 50
 * /husky_velocity_controller/right_wheel: ['front_right_whe...
 * /husky_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/type: diff_drive_contro...
 * /husky_velocity_controller/velocity_rolling_window_size: 2
 * /husky_velocity_controller/wheel_radius_multiplier: 1.0
 * /husky_velocity_controller/wheel_separation_multiplier: 1.875
 * /joy_teleop/joy_node/autorepeat_rate: 20
 * /joy_teleop/joy_node/deadzone: 0.1
 * /joy_teleop/joy_node/dev: /dev/input/ps4
 * /joy_teleop/teleop_twist_joy/axis_angular: 0
 * /joy_teleop/teleop_twist_joy/axis_linear: 1
 * /joy_teleop/teleop_twist_joy/enable_button: 4
 * /joy_teleop/teleop_twist_joy/enable_turbo_button: 5
 * /joy_teleop/teleop_twist_joy/scale_angular: 1.4
 * /joy_teleop/teleop_twist_joy/scale_linear: 0.4
 * /joy_teleop/teleop_twist_joy/scale_linear_turbo: 2.0
 * /kinova_arm_driver/arm: gen3_lite
 * /kinova_arm_driver/cyclic_data_publish_rate: 40
 * /kinova_arm_driver/dof: 6
 * /kinova_arm_driver/gripper: gen3_lite_2f
 * /kinova_arm_driver/gripper_joint_limits_max: [-0.09]
 * /kinova_arm_driver/gripper_joint_limits_min: [0.96]
 * /kinova_arm_driver/gripper_joint_names: ['right_finger_bo...
 * /kinova_arm_driver/joint_names: ['joint_1', 'join...
 * /kinova_arm_driver/maximum_accelerations: [1.0, 0.5, 0.4, 1...
 * /kinova_arm_driver/maximum_angular_acceleration: 0.4
 * /kinova_arm_driver/maximum_angular_velocity: 0.85
 * /kinova_arm_driver/maximum_linear_acceleration: 0.4
 * /kinova_arm_driver/maximum_linear_velocity: 0.3
 * /kinova_arm_driver/maximum_velocities: [0.5, 0.5, 0.5, 0...
 * /kinova_arm_driver/prefix: kinova_arm_
 * /kinova_arm_driver/robot_name: 
 * /kinova_arm_driver/sim: True
 * /kinova_arm_gen3_lite_2f_gripper_controller/action_monitor_rate: 100
 * /kinova_arm_gen3_lite_2f_gripper_controller/joint: kinova_arm_right_...
 * /kinova_arm_gen3_lite_2f_gripper_controller/type: position_controll...
 * /kinova_arm_gen3_lite_joint_trajectory_controller/action_monitor_rate: 25
 * /kinova_arm_gen3_lite_joint_trajectory_controller/constraints/goal_time: 1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/constraints/stopped_velocity_tolerance: 0.5
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/d: 2.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/i: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/i_clamp_max: 100.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/i_clamp_min: -100.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/p: 3000.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/d: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/i: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/i_clamp_max: 5.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/i_clamp_min: -5.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/p: 50000.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/d: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/i: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/i_clamp_max: 1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/i_clamp_min: -1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/p: 50000.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/d: 0.2
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/i: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/i_clamp_max: 1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/i_clamp_min: -1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/p: 750.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/d: 1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/i: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/i_clamp_max: 1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/i_clamp_min: -1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/p: 5000.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/d: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/i: 0.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/i_clamp_max: 0.1
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/i_clamp_min: -0.1
 * /kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/p: 100.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/joints: ['kinova_arm_join...
 * /kinova_arm_gen3_lite_joint_trajectory_controller/state_publish_rate: 25
 * /kinova_arm_gen3_lite_joint_trajectory_controller/stop_trajectory_duration: 1.0
 * /kinova_arm_gen3_lite_joint_trajectory_controller/type: effort_controller...
 * /kinova_arm_joint_1_position_controller/joint: kinova_arm_joint_1
 * /kinova_arm_joint_1_position_controller/pid/d: 2.0
 * /kinova_arm_joint_1_position_controller/pid/i: 0.0
 * /kinova_arm_joint_1_position_controller/pid/p: 3000.0
 * /kinova_arm_joint_1_position_controller/type: effort_controller...
 * /kinova_arm_joint_2_position_controller/joint: kinova_arm_joint_2
 * /kinova_arm_joint_2_position_controller/pid/d: 0.0
 * /kinova_arm_joint_2_position_controller/pid/i: 0.0
 * /kinova_arm_joint_2_position_controller/pid/p: 50000.0
 * /kinova_arm_joint_2_position_controller/type: effort_controller...
 * /kinova_arm_joint_3_position_controller/joint: kinova_arm_joint_3
 * /kinova_arm_joint_3_position_controller/pid/d: 0.0
 * /kinova_arm_joint_3_position_controller/pid/i: 0.0
 * /kinova_arm_joint_3_position_controller/pid/p: 50000.0
 * /kinova_arm_joint_3_position_controller/type: effort_controller...
 * /kinova_arm_joint_4_position_controller/joint: kinova_arm_joint_4
 * /kinova_arm_joint_4_position_controller/pid/d: 0.2
 * /kinova_arm_joint_4_position_controller/pid/i: 0.0
 * /kinova_arm_joint_4_position_controller/pid/p: 750.0
 * /kinova_arm_joint_4_position_controller/type: effort_controller...
 * /kinova_arm_joint_5_position_controller/joint: kinova_arm_joint_5
 * /kinova_arm_joint_5_position_controller/pid/d: 1.0
 * /kinova_arm_joint_5_position_controller/pid/i: 0.0
 * /kinova_arm_joint_5_position_controller/pid/p: 5000.0
 * /kinova_arm_joint_5_position_controller/type: effort_controller...
 * /kinova_arm_joint_6_position_controller/joint: kinova_arm_joint_6
 * /kinova_arm_joint_6_position_controller/pid/d: 0.0
 * /kinova_arm_joint_6_position_controller/pid/i: 0.0
 * /kinova_arm_joint_6_position_controller/pid/p: 100.0
 * /kinova_arm_joint_6_position_controller/type: effort_controller...
 * /kinova_arm_joint_state_controller/publish_rate: 50
 * /kinova_arm_joint_state_controller/type: joint_state_contr...
 * /move_group/allow_trajectory_execution: True
 * /move_group/arm/default_planner_config: RRTConnect
 * /move_group/arm/longest_valid_segment_fraction: 0.005
 * /move_group/arm/planner_configs: ['SBL', 'EST', 'L...
 * /move_group/arm/projection_evaluator: joints(kinova_arm...
 * /move_group/capabilities: 
 * /move_group/controller_list: [{'name': 'kinova...
 * /move_group/disable_capabilities: 
 * /move_group/gripper/planner_configs: ['SBL', 'EST', 'L...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/joint_state_controller/publish_rate: 50
 * /move_group/joint_state_controller/type: joint_state_contr...
 * /move_group/max_range: 5.0
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/octomap_resolution: 0.025
 * /move_group/planner_configs/BFMT/balanced: 0
 * /move_group/planner_configs/BFMT/cache_cc: 1
 * /move_group/planner_configs/BFMT/extended_fmt: 1
 * /move_group/planner_configs/BFMT/heuristics: 1
 * /move_group/planner_configs/BFMT/nearest_k: 1
 * /move_group/planner_configs/BFMT/num_samples: 1000
 * /move_group/planner_configs/BFMT/optimality: 1
 * /move_group/planner_configs/BFMT/radius_multiplier: 1.0
 * /move_group/planner_configs/BFMT/type: geometric::BFMT
 * /move_group/planner_configs/BKPIECE/border_fraction: 0.9
 * /move_group/planner_configs/BKPIECE/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/BKPIECE/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/BKPIECE/range: 0.0
 * /move_group/planner_configs/BKPIECE/type: geometric::BKPIECE
 * /move_group/planner_configs/BiEST/range: 0.0
 * /move_group/planner_configs/BiEST/type: geometric::BiEST
 * /move_group/planner_configs/BiTRRT/cost_threshold: 1e300
 * /move_group/planner_configs/BiTRRT/frountier_node_ratio: 0.1
 * /move_group/planner_configs/BiTRRT/frountier_threshold: 0.0
 * /move_group/planner_configs/BiTRRT/init_temperature: 100
 * /move_group/planner_configs/BiTRRT/range: 0.0
 * /move_group/planner_configs/BiTRRT/temp_change_factor: 0.1
 * /move_group/planner_configs/BiTRRT/type: geometric::BiTRRT
 * /move_group/planner_configs/EST/goal_bias: 0.05
 * /move_group/planner_configs/EST/range: 0.0
 * /move_group/planner_configs/EST/type: geometric::EST
 * /move_group/planner_configs/FMT/cache_cc: 1
 * /move_group/planner_configs/FMT/extended_fmt: 1
 * /move_group/planner_configs/FMT/heuristics: 0
 * /move_group/planner_configs/FMT/nearest_k: 1
 * /move_group/planner_configs/FMT/num_samples: 1000
 * /move_group/planner_configs/FMT/radius_multiplier: 1.1
 * /move_group/planner_configs/FMT/type: geometric::FMT
 * /move_group/planner_configs/KPIECE/border_fraction: 0.9
 * /move_group/planner_configs/KPIECE/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/KPIECE/goal_bias: 0.05
 * /move_group/planner_configs/KPIECE/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/KPIECE/range: 0.0
 * /move_group/planner_configs/KPIECE/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECE/border_fraction: 0.9
 * /move_group/planner_configs/LBKPIECE/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/LBKPIECE/range: 0.0
 * /move_group/planner_configs/LBKPIECE/type: geometric::LBKPIECE
 * /move_group/planner_configs/LBTRRT/epsilon: 0.4
 * /move_group/planner_configs/LBTRRT/goal_bias: 0.05
 * /move_group/planner_configs/LBTRRT/range: 0.0
 * /move_group/planner_configs/LBTRRT/type: geometric::LBTRRT
 * /move_group/planner_configs/LazyPRM/range: 0.0
 * /move_group/planner_configs/LazyPRM/type: geometric::LazyPRM
 * /move_group/planner_configs/LazyPRMstar/type: geometric::LazyPR...
 * /move_group/planner_configs/PDST/type: geometric::PDST
 * /move_group/planner_configs/PRM/max_nearest_neighbors: 10
 * /move_group/planner_configs/PRM/type: geometric::PRM
 * /move_group/planner_configs/PRMstar/type: geometric::PRMstar
 * /move_group/planner_configs/ProjEST/goal_bias: 0.05
 * /move_group/planner_configs/ProjEST/range: 0.0
 * /move_group/planner_configs/ProjEST/type: geometric::ProjEST
 * /move_group/planner_configs/RRT/goal_bias: 0.05
 * /move_group/planner_configs/RRT/range: 0.0
 * /move_group/planner_configs/RRT/type: geometric::RRT
 * /move_group/planner_configs/RRTConnect/range: 0.0
 * /move_group/planner_configs/RRTConnect/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTstar/delay_collision_checking: 1
 * /move_group/planner_configs/RRTstar/goal_bias: 0.05
 * /move_group/planner_configs/RRTstar/range: 0.0
 * /move_group/planner_configs/RRTstar/type: geometric::RRTstar
 * /move_group/planner_configs/SBL/range: 0.0
 * /move_group/planner_configs/SBL/type: geometric::SBL
 * /move_group/planner_configs/SPARS/dense_delta_fraction: 0.001
 * /move_group/planner_configs/SPARS/max_failures: 1000
 * /move_group/planner_configs/SPARS/sparse_delta_fraction: 0.25
 * /move_group/planner_configs/SPARS/stretch_factor: 3.0
 * /move_group/planner_configs/SPARS/type: geometric::SPARS
 * /move_group/planner_configs/SPARStwo/dense_delta_fraction: 0.001
 * /move_group/planner_configs/SPARStwo/max_failures: 5000
 * /move_group/planner_configs/SPARStwo/sparse_delta_fraction: 0.25
 * /move_group/planner_configs/SPARStwo/stretch_factor: 3.0
 * /move_group/planner_configs/SPARStwo/type: geometric::SPARStwo
 * /move_group/planner_configs/STRIDE/degree: 16
 * /move_group/planner_configs/STRIDE/estimated_dimension: 0.0
 * /move_group/planner_configs/STRIDE/goal_bias: 0.05
 * /move_group/planner_configs/STRIDE/max_degree: 18
 * /move_group/planner_configs/STRIDE/max_pts_per_leaf: 6
 * /move_group/planner_configs/STRIDE/min_degree: 12
 * /move_group/planner_configs/STRIDE/min_valid_path_fraction: 0.2
 * /move_group/planner_configs/STRIDE/range: 0.0
 * /move_group/planner_configs/STRIDE/type: geometric::STRIDE
 * /move_group/planner_configs/STRIDE/use_projected_distance: 0
 * /move_group/planner_configs/TRRT/frountierNodeRatio: 0.1
 * /move_group/planner_configs/TRRT/frountier_threshold: 0.0
 * /move_group/planner_configs/TRRT/goal_bias: 0.05
 * /move_group/planner_configs/TRRT/init_temperature: 10e-6
 * /move_group/planner_configs/TRRT/k_constant: 0.0
 * /move_group/planner_configs/TRRT/max_states_failed: 10
 * /move_group/planner_configs/TRRT/min_temperature: 10e-10
 * /move_group/planner_configs/TRRT/range: 0.0
 * /move_group/planner_configs/TRRT/temp_change_factor: 2.0
 * /move_group/planner_configs/TRRT/type: geometric::TRRT
 * /move_group/planning_plugin: ompl_interface/OM...
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: default_planner_r...
 * /move_group/sensors: [{}]
 * /move_group/start_state_max_bounds_error: 0.1
 * /move_group/trajectory_execution/allowed_execution_duration_scaling: 1.2
 * /move_group/trajectory_execution/allowed_goal_duration_margin: 0.5
 * /move_group/trajectory_execution/allowed_start_tolerance: 0.01
 * /robot_description: <?xml version="1....
 * /robot_description_kinematics/arm/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arm/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm/kinematics_solver_timeout: 0.005
 * /robot_description_planning/cartesian_limits/max_rot_vel: 1.57
 * /robot_description_planning/cartesian_limits/max_trans_acc: 2.25
 * /robot_description_planning/cartesian_limits/max_trans_dec: -5
 * /robot_description_planning/cartesian_limits/max_trans_vel: 1
 * /robot_description_planning/joint_limits/kinova_arm_joint_1/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_joint_1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_joint_1/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_joint_1/max_velocity: 1.6
 * /robot_description_planning/joint_limits/kinova_arm_joint_2/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_joint_2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_joint_2/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_joint_2/max_velocity: 1.6
 * /robot_description_planning/joint_limits/kinova_arm_joint_3/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_joint_3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_joint_3/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_joint_3/max_velocity: 1.6
 * /robot_description_planning/joint_limits/kinova_arm_joint_4/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_joint_4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_joint_4/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_joint_4/max_velocity: 1.6
 * /robot_description_planning/joint_limits/kinova_arm_joint_5/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_joint_5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_joint_5/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_joint_5/max_velocity: 1.6
 * /robot_description_planning/joint_limits/kinova_arm_joint_6/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_joint_6/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_joint_6/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_joint_6/max_velocity: 3.2
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/max_velocity: 1000
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/max_velocity: 1000
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/max_velocity: 0.6
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/max_velocity: 1000
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: noetic
 * /rosversion: 1.15.13
 * /twist_mux/locks: [{'name': 'e_stop...
 * /twist_mux/topics: [{'name': 'joy', ...
 * /use_sim_time: True

NODES
  /
    base_controller_spawner (controller_manager/spawner)
    ekf_localization (robot_localization/ekf_localization_node)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    husky_controller_spawner (controller_manager/spawner)
    husky_position_controllers_spawner (controller_manager/spawner)
    kinova_arm_driver (kortex_driver/kortex_arm_driver)
    move_group (moveit_ros_move_group/move_group)
    robot_base_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz_ricks_Alienware_m15_R6_1502516_5691912370307030918 (rviz/rviz)
    twist_marker_server (interactive_marker_twist_server/marker_server)
    twist_mux (twist_mux/twist_mux)
    urdf_spawner (gazebo_ros/spawn_model)
  /joy_teleop/
    joy_node (joy/joy_node)
    teleop_twist_joy (teleop_twist_joy/teleop_node)

auto-starting new master
process[master]: started with pid [1502603]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to be55b2e2-6ca6-11ec-9974-4d8c6a873cd5
process[rosout-1]: started with pid [1502644]
started core service [/rosout]
process[gazebo-2]: started with pid [1502651]
process[gazebo_gui-3]: started with pid [1502656]
process[urdf_spawner-4]: started with pid [1502661]
process[base_controller_spawner-5]: started with pid [1502662]
process[husky_controller_spawner-6]: started with pid [1502663]
process[husky_position_controllers_spawner-7]: started with pid [1502664]
process[kinova_arm_driver-8]: started with pid [1502665]
process[ekf_localization-9]: started with pid [1502666]
process[twist_marker_server-10]: started with pid [1502667]
process[robot_base_state_publisher-11]: started with pid [1502672]
process[twist_mux-12]: started with pid [1502695]
[ INFO] [1641222396.865599190]: [twist_marker_server] Initialized.
process[joy_teleop/joy_node-13]: started with pid [1502701]
process[joy_teleop/teleop_twist_joy-14]: started with pid [1502715]
[ERROR] [1641222396.895207508]: Couldn't open joystick /dev/input/ps4. Will retry every second.
process[move_group-15]: started with pid [1502734]
process[rviz_ricks_Alienware_m15_R6_1502516_5691912370307030918-16]: started with pid [1502771]
[ INFO] [1641222396.950180671]: -------------------------------------------------
[ INFO] [1641222396.951018179]: Initializing Kortex Driver's services...
[ WARN] [1641222396.955330421]: MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default planning pipeline configuration
[ WARN] [1641222396.956246062]: Falling back to using the the move_group node namespace (deprecated behavior).
[ INFO] [1641222396.967855427]: Loading robot model 'husky'...
[ INFO] [1641222396.967942023]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1641222396.968158688]: Link front_bumper_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ WARN] [1641222396.968228614]: Link rear_bumper_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ WARN] [1641222396.968274624]: Link top_chassis_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ INFO] [1641222397.065695662]: rviz version 1.14.10
[ INFO] [1641222397.065746032]: compiled against Qt version 5.12.8
[ INFO] [1641222397.065756894]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1641222397.073197531]: Forcing OpenGl version 0.
[ WARN] [1641222397.171537453]: Link velodyne has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ WARN] [1641222397.171578635]: Link user_rail_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ WARN] [1641222397.171924330]: Could not identify parent group for end-effector 'end_effector'
[INFO] [1641222397.235200, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1641222397.245025, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[INFO] [1641222397.259673, 0.000000]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1641222397.289703, 0.000000]: Controller Spawner: Waiting for service controller_manager/load_controller
[ INFO] [1641222397.427839807]: Stereo is NOT SUPPORTED
[ INFO] [1641222397.427906666]: OpenGL device: NVIDIA GeForce RTX 3080 Laptop GPU/PCIe/SSE2
[ INFO] [1641222397.427919041]: OpenGl version: 4,6 (GLSL 4,6).
[ INFO] [1641222397.460444018]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1641222397.461096466]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1641222397.532312183]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1641222397.532946420]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1641222397.733772667]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1641222397.734972918]: Listening to 'joint_states' for joint states
[ INFO] [1641222397.736954755]: Listening to '/attached_collision_object' for attached collision objects
[ INFO] [1641222397.736969115]: Starting planning scene monitor
[ INFO] [1641222397.737876477]: Listening to '/planning_scene'
[ INFO] [1641222397.737888171]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1641222397.738608843]: Listening to '/collision_object'
[ INFO] [1641222397.739291054]: Listening to '/planning_scene_world' for planning scene world geometry
[ERROR] [1641222397.739569949]: No sensor plugin specified for octomap updater 0; ignoring.
[ INFO] [1641222398.100886022]: Loading planning pipeline ''
[ INFO] [1641222398.133485573]: Using planning interface 'OMPL'
[ INFO] [1641222398.135989873]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1641222398.136169385]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1641222398.136330343]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1641222398.136512729]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1641222398.136668947]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1641222398.136847104]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1641222398.136864296]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1641222398.136874909]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1641222398.136882999]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1641222398.136891332]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1641222398.136899481]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1641222398.212562794, 0.012000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1641222398.254082649, 0.052000000]: Physics dynamic reconfigure ready.
[ WARN] [1641222398.419068030, 0.213000000]: Failed to meet update rate! Took 0.21300000000000002265
[ WARN] [1641222398.420066590, 0.214000000]: Failed to meet update rate! Took 0.19400000000000000577
[INFO] [1641222398.450143, 0.244000]: Calling service /gazebo/spawn_urdf_model
[ INFO] [1641222399.024925631, 0.378000000]: Kortex Driver's services initialized correctly.
[ INFO] [1641222399.024968424, 0.378000000]: -------------------------------------------------
[kinova_arm_driver-8] process has died [pid 1502665, exit code -11, cmd /home/yujie/robot_pushing/pushing_training/noetic_husky_2/devel/lib/kortex_driver/kortex_arm_driver __name:=kinova_arm_driver __log:=/home/yujie/.ros/log/be55b2e2-6ca6-11ec-9974-4d8c6a873cd5/kinova_arm_driver-8.log].
log file: /home/yujie/.ros/log/be55b2e2-6ca6-11ec-9974-4d8c6a873cd5/kinova_arm_driver-8*.log
Topic [///joint_cmd] is not valid.
Service [///joint_cmd_req] is not valid.
[INFO] [1641222409.850316, 0.378000]: Spawn status: SpawnModel: Successfully spawned entity
[INFO] [1641222409.851264, 0.378000]: Waiting for service /gazebo/set_model_configuration
[INFO] [1641222409.853025, 0.378000]: temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition.
[ INFO] [1641222409.859874005, 0.378000000]: Velodyne laser plugin missing <min_intensity>, defaults to no clipping
[ INFO] [1641222409.860759525, 0.378000000]: Velodyne laser plugin ready, 32 lasers
[ INFO] [1641222410.036428817, 0.378000000]: Loading gazebo_ros_control plugin
[ INFO] [1641222410.036510467, 0.378000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1641222410.037214313, 0.378000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1641222410.156705303, 0.378000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_left_wheel
[ERROR] [1641222410.157218843, 0.378000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_right_wheel
[ERROR] [1641222410.157618562, 0.378000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_left_wheel
[ERROR] [1641222410.157945166, 0.378000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_right_wheel
[ INFO] [1641222410.167208518, 0.378000000]: Loaded gazebo_ros_control.
[ INFO] [1641222410.172243160, 0.378000000]: imu plugin missing <xyzOffset>, defaults to 0s
[ INFO] [1641222410.172259337, 0.378000000]: imu plugin missing <rpyOffset>, defaults to 0s
[INFO] [1641222410.200240, 0.000000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1641222410.201975, 0.000000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1641222410.203606, 0.000000]: Loading controller: kinova_arm_joint_1_position_controller
[ INFO] [1641222410.208566733, 0.378000000]: MimicJointPlugin loaded! Joint: "kinova_arm_right_finger_bottom_joint", Mimic joint: "kinova_arm_right_finger_tip_joint", Multiplier: -0.676, Offset: 0.149, MaxEffort: 5, Sensitiveness: 0
[ INFO] [1641222410.220788519, 0.378000000]: MimicJointPlugin loaded! Joint: "kinova_arm_right_finger_bottom_joint", Mimic joint: "kinova_arm_left_finger_bottom_joint", Multiplier: -1, Offset: 0, MaxEffort: 5, Sensitiveness: 0
[INFO] [1641222410.225285, 0.000000]: Loading controller: kinova_arm_joint_2_position_controller
[INFO] [1641222410.228810, 0.000000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1641222410.231061, 0.000000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[ INFO] [1641222410.232187980, 0.378000000]: MimicJointPlugin loaded! Joint: "kinova_arm_right_finger_bottom_joint", Mimic joint: "kinova_arm_left_finger_tip_joint", Multiplier: -0.676, Offset: 0.149, MaxEffort: 5, Sensitiveness: 0
[INFO] [1641222410.232900, 0.000000]: Loading controller: kinova_arm_gen3_lite_joint_trajectory_controller
[Msg] Loading grasp-fix plugin
[Msg] GazeboGraspFix: Using disable_collisions_on_attach 0
[Msg] GazeboGraspFix: Using update rate 10
[Msg] GazeboGraspFix: Using max_grip_count 10
[Msg] GazeboGraspFix: Using grip_count_threshold 3
[Msg] GazeboGraspFix: Using release_tolerance 0.001
[INFO] [1641222410.240539, 0.000000]: Loading controller: kinova_arm_joint_3_position_controller
[Msg] GazeboGraspFix: Adding collision scoped name /::kinova_arm_left_finger_dist_link::kinova_arm_left_finger_dist_link_collision
[Msg] GazeboGraspFix: Adding collision scoped name /::kinova_arm_right_finger_dist_link::kinova_arm_right_finger_dist_link_collision
[Msg] Subscribing contact manager to topic ~///contacts
[INFO] [1641222410.302432, 0.439000]: Loading controller: kinova_arm_joint_state_controller
[INFO] [1641222410.315354, 0.452000]: Loading controller: kinova_arm_joint_4_position_controller
[INFO] [1641222410.317331, 0.454000]: Loading controller: kinova_arm_gen3_lite_2f_gripper_controller
[ INFO] [1641222410.321356498, 0.458000000]: Controller state will be published at 50Hz.
[ INFO] [1641222410.321787196, 0.458000000]: Wheel separation will be multiplied by 1.875.
[ INFO] [1641222410.322023903, 0.458000000]: Left wheel radius will be multiplied by 1.
[ INFO] [1641222410.322038099, 0.458000000]: Right wheel radius will be multiplied by 1.
[ INFO] [1641222410.322290505, 0.459000000]: Velocity rolling window size of 2.
[ INFO] [1641222410.322558290, 0.459000000]: Velocity commands will be considered old if they are older than 0.25s.
[ INFO] [1641222410.322682223, 0.459000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1641222410.322911974, 0.459000000]: Base frame_id set to base_link
[ INFO] [1641222410.323026980, 0.459000000]: Odometry frame_id set to odom
[ INFO] [1641222410.323290812, 0.460000000]: Publishing to tf is disabled
[ INFO] [1641222410.327798109, 0.464000000]: left wheel to origin: 0.256,0.2854, 0.03282
[ INFO] [1641222410.327833880, 0.464000000]: right wheel to origin: 0.256,-0.2854, 0.03282
[ INFO] [1641222410.327892965, 0.464000000]: Odometry params : wheel separation 1.07025, left wheel radius 0.1651, right wheel radius 0.1651
[ INFO] [1641222410.328943873, 0.465000000]: Adding left wheel with joint name: front_left_wheel and right wheel with joint name: front_right_wheel
[ INFO] [1641222410.328977150, 0.465000000]: Adding left wheel with joint name: rear_left_wheel and right wheel with joint name: rear_right_wheel
[ INFO] [1641222410.332778095, 0.469000000]: Dynamic Reconfigure:
DynamicParams:
	Odometry parameters:
		left wheel radius multiplier: 1
		right wheel radius multiplier: 1
		wheel separation multiplier: 1.875
	Publication parameters:
		Publish executed velocity command: disabled
		Publication rate: 50
		Publish frame odom on tf: disabled
[INFO] [1641222410.348879, 0.485000]: Loading controller: kinova_arm_joint_5_position_controller
[INFO] [1641222410.362025, 0.498000]: Controller Spawner: Loaded controllers: kinova_arm_gen3_lite_joint_trajectory_controller, kinova_arm_joint_state_controller, kinova_arm_gen3_lite_2f_gripper_controller
[ INFO] [1641222410.549684569, 0.501000000]: Added FollowJointTrajectory controller for kinova_arm_gen3_lite_joint_trajectory_controller
[INFO] [1641222410.667831, 0.502000]: Loading controller: kinova_arm_joint_6_position_controller
[INFO] [1641222410.668720, 0.503000]: Started controllers: kinova_arm_gen3_lite_joint_trajectory_controller, kinova_arm_joint_state_controller, kinova_arm_gen3_lite_2f_gripper_controller
[INFO] [1641222410.680633, 0.515000]: Controller Spawner: Loaded controllers: kinova_arm_joint_1_position_controller, kinova_arm_joint_2_position_controller, kinova_arm_joint_3_position_controller, kinova_arm_joint_4_position_controller, kinova_arm_joint_5_position_controller, kinova_arm_joint_6_position_controller
[ INFO] [1641222410.803034434, 0.600000000]: Added GripperCommand controller for kinova_arm_gen3_lite_2f_gripper_controller
[ INFO] [1641222410.803096041, 0.600000000]: Returned 2 controllers in list
[ INFO] [1641222410.807877279, 0.600000000]: Trajectory execution is managing controllers
[ INFO] [1641222410.807898188, 0.600000000]: MoveGroup debug mode is OFF
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1641222410.835433837, 0.600000000]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1641222410.835476137, 0.600000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1641222410.835487115, 0.600000000]: MoveGroup context initialization complete

You can start planning now!

[INFO] [1641222410.854403, 0.600000]: Calling service /gazebo/set_model_configuration
[INFO] [1641222411.058677, 0.600000]: Set model configuration status: SetModelConfiguration: success
[urdf_spawner-4] process has finished cleanly
log file: /home/yujie/.ros/log/be55b2e2-6ca6-11ec-9974-4d8c6a873cd5/urdf_spawner-4*.log

The output of the rostopic list is:

/action_topic
/arm_state_topic
/attached_collision_object
/clicked_point
/clock
/cmd_vel
/collision_object
/configuration_change_topic
/control_configuration_topic
/control_mode_topic
/controller_topic
/diagnostics
/e_stop
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/factory_topic
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/parameter_descriptions
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/parameter_updates
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/parameter_descriptions
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/parameter_updates
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/parameter_descriptions
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/parameter_updates
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/parameter_descriptions
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/parameter_updates
/husky_velocity_controller/cmd_vel
/husky_velocity_controller/odom
/husky_velocity_controller/parameter_descriptions
/husky_velocity_controller/parameter_updates
/imu/data
/imu/data/accel/parameter_descriptions
/imu/data/accel/parameter_updates
/imu/data/bias
/imu/data/rate/parameter_descriptions
/imu/data/rate/parameter_updates
/imu/data/yaw/parameter_descriptions
/imu/data/yaw/parameter_updates
/initialpose
/joint_states
/joy_teleop/cmd_vel
/joy_teleop/joy
/joy_teleop/joy/set_feedback
/kinova_arm_gen3_lite_2f_gripper_controller/gripper_cmd/cancel
/kinova_arm_gen3_lite_2f_gripper_controller/gripper_cmd/feedback
/kinova_arm_gen3_lite_2f_gripper_controller/gripper_cmd/goal
/kinova_arm_gen3_lite_2f_gripper_controller/gripper_cmd/result
/kinova_arm_gen3_lite_2f_gripper_controller/gripper_cmd/status
/kinova_arm_gen3_lite_joint_trajectory_controller/command
/kinova_arm_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/cancel
/kinova_arm_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/feedback
/kinova_arm_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/goal
/kinova_arm_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result
/kinova_arm_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/status
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/parameter_descriptions
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/parameter_updates
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/parameter_descriptions
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/parameter_updates
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/parameter_descriptions
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/parameter_updates
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/parameter_descriptions
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/parameter_updates
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/parameter_descriptions
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/parameter_updates
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/parameter_descriptions
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/parameter_updates
/kinova_arm_gen3_lite_joint_trajectory_controller/state
/kinova_arm_joint_1_position_controller/command
/kinova_arm_joint_1_position_controller/pid/parameter_descriptions
/kinova_arm_joint_1_position_controller/pid/parameter_updates
/kinova_arm_joint_1_position_controller/state
/kinova_arm_joint_2_position_controller/command
/kinova_arm_joint_2_position_controller/pid/parameter_descriptions
/kinova_arm_joint_2_position_controller/pid/parameter_updates
/kinova_arm_joint_2_position_controller/state
/kinova_arm_joint_3_position_controller/command
/kinova_arm_joint_3_position_controller/pid/parameter_descriptions
/kinova_arm_joint_3_position_controller/pid/parameter_updates
/kinova_arm_joint_3_position_controller/state
/kinova_arm_joint_4_position_controller/command
/kinova_arm_joint_4_position_controller/pid/parameter_descriptions
/kinova_arm_joint_4_position_controller/pid/parameter_updates
/kinova_arm_joint_4_position_controller/state
/kinova_arm_joint_5_position_controller/command
/kinova_arm_joint_5_position_controller/pid/parameter_descriptions
/kinova_arm_joint_5_position_controller/pid/parameter_updates
/kinova_arm_joint_5_position_controller/state
/kinova_arm_joint_6_position_controller/command
/kinova_arm_joint_6_position_controller/pid/parameter_descriptions
/kinova_arm_joint_6_position_controller/pid/parameter_updates
/kinova_arm_joint_6_position_controller/state
/kortex_error
/mapping_info_topic
/move_base_simple/goal
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/navsat/fix
/navsat/fix/position/parameter_descriptions
/navsat/fix/position/parameter_updates
/navsat/fix/status/parameter_descriptions
/navsat/fix/status/parameter_updates
/navsat/fix/velocity/parameter_descriptions
/navsat/fix/velocity/parameter_updates
/navsat/vel
/network_topic
/odometry/filtered
/operating_mode_topic
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/protection_zone_topic
/robot_event_topic
/rosout
/rosout_agg
/safety_topic
/sequence_info_topic
/servoing_mode_topic
/set_pose
/tf
/tf_static
/trajectory_execution_event
/twist_marker_server/cmd_vel
/twist_marker_server/feedback
/twist_marker_server/update
/twist_marker_server/update_full
/user_topic
/velodyne_points

@felixmaisonneuve
Copy link
Contributor

Hi @yj-Tang,

I have looked at what you sent me. This indeed looks like the same issue. Your simulation driver is not initializing correctly somewhere in this function :

KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle),

What arguments do you use with your roslaunch command (e.g. you need to specify arm:=gen3_lite gripper:=gen3_lite_2f at some point)? What other arguments do you use? Do you change prefix and name also? These could be specified in a launch file.

Can you send the output of rosparam dump?

Best,
Felix

@felixmaisonneuve
Copy link
Contributor

Looking at the rosparam dump output of @civerachb-cpr, the robot_description is not in the relative param directory (/arm/arm_driver), it is directly in the root param directory, so I think changing this line

model.initParam("/" + m_robot_name + "/robot_description");

to

model.initParam("/robot_description");

might allow the simulation driver to find the robot_description so it will initialize correctly and possibly fix the issue.

@civerachb-cpr
Copy link
Author

Sorry about the late reply; just got back from vacation. No, I have not yet been able to fix this problem yet either.

@yj-Tang
Copy link

yj-Tang commented Jan 6, 2022

Looking at the rosparam dump output of @civerachb-cpr, the robot_description is not in the relative param directory (/arm/arm_driver), it is directly in the root param directory, so I think changing this line

model.initParam("/" + m_robot_name + "/robot_description");

to

model.initParam("/robot_description");

might allow the simulation driver to find the robot_description so it will initialize correctly and possibly fix the issue.

Hi, @felixmaisonneuve. Thanks for your quick reply. I have changed the scripts as you recommend. But it still makes no difference. The output of param is shown below

/camera_description
/ekf_localization/base_link_frame
/ekf_localization/frequency
/ekf_localization/imu0
/ekf_localization/imu0_config
/ekf_localization/imu0_differential
/ekf_localization/imu0_queue_size
/ekf_localization/imu0_remove_gravitational_acceleration
/ekf_localization/odom0
/ekf_localization/odom0_config
/ekf_localization/odom0_differential
/ekf_localization/odom0_queue_size
/ekf_localization/odom_frame
/ekf_localization/two_d_mode
/ekf_localization/world_frame
/gazebo/auto_disable_bodies
/gazebo/cfm
/gazebo/contact_max_correcting_vel
/gazebo/contact_surface_layer
/gazebo/enable_ros_network
/gazebo/erp
/gazebo/gravity_x
/gazebo/gravity_y
/gazebo/gravity_z
/gazebo/max_contacts
/gazebo/max_update_rate
/gazebo/sor_pgs_iters
/gazebo/sor_pgs_precon_iters
/gazebo/sor_pgs_rms_error_tol
/gazebo/sor_pgs_w
/gazebo/time_step
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/antiwindup
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/d
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/i
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/i_clamp_max
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/i_clamp_min
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_bottom_joint/p
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/antiwindup
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/d
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/i
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/i_clamp_max
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/i_clamp_min
/gazebo_ros_control/pid_gains/kinova_arm_left_finger_tip_joint/p
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/antiwindup
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/d
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/i
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/i_clamp_max
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/i_clamp_min
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_bottom_joint/p
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/antiwindup
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/d
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/i
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/i_clamp_max
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/i_clamp_min
/gazebo_ros_control/pid_gains/kinova_arm_right_finger_tip_joint/p
/husky_joint_publisher/publish_rate
/husky_joint_publisher/type
/husky_velocity_controller/angular/z/has_acceleration_limits
/husky_velocity_controller/angular/z/has_velocity_limits
/husky_velocity_controller/angular/z/max_acceleration
/husky_velocity_controller/angular/z/max_velocity
/husky_velocity_controller/base_frame_id
/husky_velocity_controller/cmd_vel_timeout
/husky_velocity_controller/enable_odom_tf
/husky_velocity_controller/estimate_velocity_from_position
/husky_velocity_controller/left_wheel
/husky_velocity_controller/left_wheel_radius_multiplier
/husky_velocity_controller/linear/x/has_acceleration_limits
/husky_velocity_controller/linear/x/has_velocity_limits
/husky_velocity_controller/linear/x/max_acceleration
/husky_velocity_controller/linear/x/max_velocity
/husky_velocity_controller/pose_covariance_diagonal
/husky_velocity_controller/publish_rate
/husky_velocity_controller/right_wheel
/husky_velocity_controller/right_wheel_radius_multiplier
/husky_velocity_controller/twist_covariance_diagonal
/husky_velocity_controller/type
/husky_velocity_controller/velocity_rolling_window_size
/husky_velocity_controller/wheel_radius_multiplier
/husky_velocity_controller/wheel_separation_multiplier
/imu/data/accel/drift
/imu/data/accel/drift_frequency
/imu/data/accel/gaussian_noise
/imu/data/accel/offset
/imu/data/accel/scale_error
/imu/data/rate/drift
/imu/data/rate/drift_frequency
/imu/data/rate/gaussian_noise
/imu/data/rate/offset
/imu/data/rate/scale_error
/imu/data/yaw/drift
/imu/data/yaw/drift_frequency
/imu/data/yaw/gaussian_noise
/imu/data/yaw/offset
/imu/data/yaw/scale_error
/is_initialized
/joy_teleop/joy_node/autorepeat_rate
/joy_teleop/joy_node/deadzone
/joy_teleop/joy_node/dev
/joy_teleop/teleop_twist_joy/axis_angular
/joy_teleop/teleop_twist_joy/axis_linear
/joy_teleop/teleop_twist_joy/enable_button
/joy_teleop/teleop_twist_joy/enable_turbo_button
/joy_teleop/teleop_twist_joy/scale_angular
/joy_teleop/teleop_twist_joy/scale_linear
/joy_teleop/teleop_twist_joy/scale_linear_turbo
/kinova_arm_driver/arm
/kinova_arm_driver/cyclic_data_publish_rate
/kinova_arm_driver/dof
/kinova_arm_driver/gripper
/kinova_arm_driver/gripper_joint_limits_max
/kinova_arm_driver/gripper_joint_limits_min
/kinova_arm_driver/gripper_joint_names
/kinova_arm_driver/joint_names
/kinova_arm_driver/maximum_accelerations
/kinova_arm_driver/maximum_angular_acceleration
/kinova_arm_driver/maximum_angular_velocity
/kinova_arm_driver/maximum_linear_acceleration
/kinova_arm_driver/maximum_linear_velocity
/kinova_arm_driver/maximum_velocities
/kinova_arm_driver/prefix
/kinova_arm_driver/robot_name
/kinova_arm_driver/sim
/kinova_arm_gen3_lite_2f_gripper_controller/action_monitor_rate
/kinova_arm_gen3_lite_2f_gripper_controller/joint
/kinova_arm_gen3_lite_2f_gripper_controller/type
/kinova_arm_gen3_lite_joint_trajectory_controller/action_monitor_rate
/kinova_arm_gen3_lite_joint_trajectory_controller/constraints/goal_time
/kinova_arm_gen3_lite_joint_trajectory_controller/constraints/stopped_velocity_tolerance
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/antiwindup
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/d
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/i
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/i_clamp_max
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/i_clamp_min
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_1/p
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/antiwindup
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/d
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/i
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/i_clamp_max
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/i_clamp_min
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_2/p
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/antiwindup
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/d
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/i
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/i_clamp_max
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/i_clamp_min
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_3/p
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/antiwindup
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/d
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/i
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/i_clamp_max
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/i_clamp_min
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_4/p
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/antiwindup
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/d
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/i
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/i_clamp_max
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/i_clamp_min
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_5/p
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/antiwindup
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/d
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/i
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/i_clamp_max
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/i_clamp_min
/kinova_arm_gen3_lite_joint_trajectory_controller/gains/kinova_arm_joint_6/p
/kinova_arm_gen3_lite_joint_trajectory_controller/joints
/kinova_arm_gen3_lite_joint_trajectory_controller/state_publish_rate
/kinova_arm_gen3_lite_joint_trajectory_controller/stop_trajectory_duration
/kinova_arm_gen3_lite_joint_trajectory_controller/type
/kinova_arm_joint_1_position_controller/joint
/kinova_arm_joint_1_position_controller/pid/antiwindup
/kinova_arm_joint_1_position_controller/pid/d
/kinova_arm_joint_1_position_controller/pid/i
/kinova_arm_joint_1_position_controller/pid/i_clamp_max
/kinova_arm_joint_1_position_controller/pid/i_clamp_min
/kinova_arm_joint_1_position_controller/pid/p
/kinova_arm_joint_1_position_controller/type
/kinova_arm_joint_2_position_controller/joint
/kinova_arm_joint_2_position_controller/pid/antiwindup
/kinova_arm_joint_2_position_controller/pid/d
/kinova_arm_joint_2_position_controller/pid/i
/kinova_arm_joint_2_position_controller/pid/i_clamp_max
/kinova_arm_joint_2_position_controller/pid/i_clamp_min
/kinova_arm_joint_2_position_controller/pid/p
/kinova_arm_joint_2_position_controller/type
/kinova_arm_joint_3_position_controller/joint
/kinova_arm_joint_3_position_controller/pid/antiwindup
/kinova_arm_joint_3_position_controller/pid/d
/kinova_arm_joint_3_position_controller/pid/i
/kinova_arm_joint_3_position_controller/pid/i_clamp_max
/kinova_arm_joint_3_position_controller/pid/i_clamp_min
/kinova_arm_joint_3_position_controller/pid/p
/kinova_arm_joint_3_position_controller/type
/kinova_arm_joint_4_position_controller/joint
/kinova_arm_joint_4_position_controller/pid/antiwindup
/kinova_arm_joint_4_position_controller/pid/d
/kinova_arm_joint_4_position_controller/pid/i
/kinova_arm_joint_4_position_controller/pid/i_clamp_max
/kinova_arm_joint_4_position_controller/pid/i_clamp_min
/kinova_arm_joint_4_position_controller/pid/p
/kinova_arm_joint_4_position_controller/type
/kinova_arm_joint_5_position_controller/joint
/kinova_arm_joint_5_position_controller/pid/antiwindup
/kinova_arm_joint_5_position_controller/pid/d
/kinova_arm_joint_5_position_controller/pid/i
/kinova_arm_joint_5_position_controller/pid/i_clamp_max
/kinova_arm_joint_5_position_controller/pid/i_clamp_min
/kinova_arm_joint_5_position_controller/pid/p
/kinova_arm_joint_5_position_controller/type
/kinova_arm_joint_6_position_controller/joint
/kinova_arm_joint_6_position_controller/pid/antiwindup
/kinova_arm_joint_6_position_controller/pid/d
/kinova_arm_joint_6_position_controller/pid/i
/kinova_arm_joint_6_position_controller/pid/i_clamp_max
/kinova_arm_joint_6_position_controller/pid/i_clamp_min
/kinova_arm_joint_6_position_controller/pid/p
/kinova_arm_joint_6_position_controller/type
/kinova_arm_joint_state_controller/publish_rate
/kinova_arm_joint_state_controller/type
/move_group/allow_trajectory_execution
/move_group/arm/default_planner_config
/move_group/arm/longest_valid_segment_fraction
/move_group/arm/planner_configs
/move_group/arm/projection_evaluator
/move_group/capabilities
/move_group/controller_list
/move_group/default_planning_pipeline
/move_group/disable_capabilities
/move_group/gripper/planner_configs
/move_group/jiggle_fraction
/move_group/joint_state_controller/publish_rate
/move_group/joint_state_controller/type
/move_group/max_range
/move_group/max_safe_path_cost
/move_group/moveit_controller_manager
/move_group/moveit_manage_controllers
/move_group/octomap_resolution
/move_group/ompl/display_random_valid_states
/move_group/ompl/link_for_exploration_tree
/move_group/ompl/maximum_waypoint_distance
/move_group/ompl/minimum_waypoint_count
/move_group/ompl/simplify_solutions
/move_group/plan_execution/max_replan_attempts
/move_group/plan_execution/record_trajectory_state_frequency
/move_group/planner_configs/BFMT/balanced
/move_group/planner_configs/BFMT/cache_cc
/move_group/planner_configs/BFMT/extended_fmt
/move_group/planner_configs/BFMT/heuristics
/move_group/planner_configs/BFMT/nearest_k
/move_group/planner_configs/BFMT/num_samples
/move_group/planner_configs/BFMT/optimality
/move_group/planner_configs/BFMT/radius_multiplier
/move_group/planner_configs/BFMT/type
/move_group/planner_configs/BKPIECE/border_fraction
/move_group/planner_configs/BKPIECE/failed_expansion_score_factor
/move_group/planner_configs/BKPIECE/min_valid_path_fraction
/move_group/planner_configs/BKPIECE/range
/move_group/planner_configs/BKPIECE/type
/move_group/planner_configs/BiEST/range
/move_group/planner_configs/BiEST/type
/move_group/planner_configs/BiTRRT/cost_threshold
/move_group/planner_configs/BiTRRT/frountier_node_ratio
/move_group/planner_configs/BiTRRT/frountier_threshold
/move_group/planner_configs/BiTRRT/init_temperature
/move_group/planner_configs/BiTRRT/range
/move_group/planner_configs/BiTRRT/temp_change_factor
/move_group/planner_configs/BiTRRT/type
/move_group/planner_configs/EST/goal_bias
/move_group/planner_configs/EST/range
/move_group/planner_configs/EST/type
/move_group/planner_configs/FMT/cache_cc
/move_group/planner_configs/FMT/extended_fmt
/move_group/planner_configs/FMT/heuristics
/move_group/planner_configs/FMT/nearest_k
/move_group/planner_configs/FMT/num_samples
/move_group/planner_configs/FMT/radius_multiplier
/move_group/planner_configs/FMT/type
/move_group/planner_configs/KPIECE/border_fraction
/move_group/planner_configs/KPIECE/failed_expansion_score_factor
/move_group/planner_configs/KPIECE/goal_bias
/move_group/planner_configs/KPIECE/min_valid_path_fraction
/move_group/planner_configs/KPIECE/range
/move_group/planner_configs/KPIECE/type
/move_group/planner_configs/LBKPIECE/border_fraction
/move_group/planner_configs/LBKPIECE/min_valid_path_fraction
/move_group/planner_configs/LBKPIECE/range
/move_group/planner_configs/LBKPIECE/type
/move_group/planner_configs/LBTRRT/epsilon
/move_group/planner_configs/LBTRRT/goal_bias
/move_group/planner_configs/LBTRRT/range
/move_group/planner_configs/LBTRRT/type
/move_group/planner_configs/LazyPRM/range
/move_group/planner_configs/LazyPRM/type
/move_group/planner_configs/LazyPRMstar/type
/move_group/planner_configs/PDST/type
/move_group/planner_configs/PRM/max_nearest_neighbors
/move_group/planner_configs/PRM/type
/move_group/planner_configs/PRMstar/type
/move_group/planner_configs/ProjEST/goal_bias
/move_group/planner_configs/ProjEST/range
/move_group/planner_configs/ProjEST/type
/move_group/planner_configs/RRT/goal_bias
/move_group/planner_configs/RRT/range
/move_group/planner_configs/RRT/type
/move_group/planner_configs/RRTConnect/range
/move_group/planner_configs/RRTConnect/type
/move_group/planner_configs/RRTstar/delay_collision_checking
/move_group/planner_configs/RRTstar/goal_bias
/move_group/planner_configs/RRTstar/range
/move_group/planner_configs/RRTstar/type
/move_group/planner_configs/SBL/range
/move_group/planner_configs/SBL/type
/move_group/planner_configs/SPARS/dense_delta_fraction
/move_group/planner_configs/SPARS/max_failures
/move_group/planner_configs/SPARS/sparse_delta_fraction
/move_group/planner_configs/SPARS/stretch_factor
/move_group/planner_configs/SPARS/type
/move_group/planner_configs/SPARStwo/dense_delta_fraction
/move_group/planner_configs/SPARStwo/max_failures
/move_group/planner_configs/SPARStwo/sparse_delta_fraction
/move_group/planner_configs/SPARStwo/stretch_factor
/move_group/planner_configs/SPARStwo/type
/move_group/planner_configs/STRIDE/degree
/move_group/planner_configs/STRIDE/estimated_dimension
/move_group/planner_configs/STRIDE/goal_bias
/move_group/planner_configs/STRIDE/max_degree
/move_group/planner_configs/STRIDE/max_pts_per_leaf
/move_group/planner_configs/STRIDE/min_degree
/move_group/planner_configs/STRIDE/min_valid_path_fraction
/move_group/planner_configs/STRIDE/range
/move_group/planner_configs/STRIDE/type
/move_group/planner_configs/STRIDE/use_projected_distance
/move_group/planner_configs/TRRT/frountierNodeRatio
/move_group/planner_configs/TRRT/frountier_threshold
/move_group/planner_configs/TRRT/goal_bias
/move_group/planner_configs/TRRT/init_temperature
/move_group/planner_configs/TRRT/k_constant
/move_group/planner_configs/TRRT/max_states_failed
/move_group/planner_configs/TRRT/min_temperature
/move_group/planner_configs/TRRT/range
/move_group/planner_configs/TRRT/temp_change_factor
/move_group/planner_configs/TRRT/type
/move_group/planning_plugin
/move_group/planning_scene_monitor/publish_geometry_updates
/move_group/planning_scene_monitor/publish_planning_scene
/move_group/planning_scene_monitor/publish_planning_scene_hz
/move_group/planning_scene_monitor/publish_state_updates
/move_group/planning_scene_monitor/publish_transforms_updates
/move_group/request_adapters
/move_group/sense_for_plan/discard_overlapping_cost_sources
/move_group/sense_for_plan/max_cost_sources
/move_group/sense_for_plan/max_look_attempts
/move_group/sense_for_plan/max_safe_path_cost
/move_group/sensors
/move_group/start_state_max_bounds_error
/move_group/trajectory_execution/allowed_execution_duration_scaling
/move_group/trajectory_execution/allowed_goal_duration_margin
/move_group/trajectory_execution/allowed_start_tolerance
/move_group/trajectory_execution/execution_duration_monitoring
/move_group/trajectory_execution/execution_velocity_scaling
/move_group/trajectory_execution/wait_for_trajectory_completion
/navsat/fix/position/drift
/navsat/fix/position/drift_frequency
/navsat/fix/position/gaussian_noise
/navsat/fix/position/offset
/navsat/fix/position/scale_error
/navsat/fix/status/SERVICE_COMPASS
/navsat/fix/status/SERVICE_GALILEO
/navsat/fix/status/SERVICE_GLONASS
/navsat/fix/status/SERVICE_GPS
/navsat/fix/status/STATUS_FIX
/navsat/fix/status/STATUS_GBAS_FIX
/navsat/fix/status/STATUS_SBAS_FIX
/navsat/fix/velocity/drift
/navsat/fix/velocity/drift_frequency
/navsat/fix/velocity/gaussian_noise
/navsat/fix/velocity/offset
/navsat/fix/velocity/scale_error
/robot_description
/robot_description_kinematics/arm/kinematics_solver
/robot_description_kinematics/arm/kinematics_solver_search_resolution
/robot_description_kinematics/arm/kinematics_solver_timeout
/robot_description_planning/cartesian_limits/max_rot_vel
/robot_description_planning/cartesian_limits/max_trans_acc
/robot_description_planning/cartesian_limits/max_trans_dec
/robot_description_planning/cartesian_limits/max_trans_vel
/robot_description_planning/joint_limits/kinova_arm_joint_1/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_joint_1/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_joint_1/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_joint_1/max_velocity
/robot_description_planning/joint_limits/kinova_arm_joint_2/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_joint_2/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_joint_2/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_joint_2/max_velocity
/robot_description_planning/joint_limits/kinova_arm_joint_3/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_joint_3/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_joint_3/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_joint_3/max_velocity
/robot_description_planning/joint_limits/kinova_arm_joint_4/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_joint_4/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_joint_4/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_joint_4/max_velocity
/robot_description_planning/joint_limits/kinova_arm_joint_5/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_joint_5/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_joint_5/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_joint_5/max_velocity
/robot_description_planning/joint_limits/kinova_arm_joint_6/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_joint_6/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_joint_6/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_joint_6/max_velocity
/robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_left_finger_bottom_joint/max_velocity
/robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_left_finger_tip_joint/max_velocity
/robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_right_finger_bottom_joint/max_velocity
/robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/has_acceleration_limits
/robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/has_velocity_limits
/robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/max_acceleration
/robot_description_planning/joint_limits/kinova_arm_right_finger_tip_joint/max_velocity
/robot_description_semantic
/rosdistro
/roslaunch/uris/host_ricks_alienware_m15_r6__46821
/rosversion
/run_id
/twist_mux/locks
/twist_mux/topics
/use_sim_time

@felixmaisonneuve
Copy link
Contributor

felixmaisonneuve commented Jan 7, 2022

At that point, I am pretty sure it gets stuck at that line :

model.initParam("/" + m_robot_name + "/robot_description");

because, based on your roslaunch output, it never reaches this section (couple lines below) :

ROS_INFO("Simulating arm with following characteristics:");
ROS_INFO("Arm type : %s", m_arm_name.c_str());
ROS_INFO("Gripper type : %s", m_gripper_name.empty() ? "None" : m_gripper_name.c_str());
ROS_INFO("Arm namespace : %s", m_robot_name.c_str());
ROS_INFO("URDF prefix : %s", m_prefix.c_str());

I would try changing the line to different variations like

model.initParam("robot_description");

and

model.initParam("~robot_description"); // this one I doubt very much

Keep in mind you need to rebuild your catkin workspace with catkin_make each time you modify something in the cpp files for the changes to take effect. Forgot to specify it last time, so if you didn't do it, you might want to try again with

model.initParam("/robot_description");

@yj-Tang
Copy link

yj-Tang commented Jan 10, 2022

I tried different variations, but it still doesn't work. However, I can simulate the arm in gazebo and use moveit to control it without the driver. So I am wondering what is the function of this driver? Can we simulate the arm without the driver?

@felixmaisonneuve
Copy link
Contributor

The driver simulates some API calls (listed here) and add ros subscribers to certain topics that uses those API calls. It also initialize the 3 pre-defined arm positions in MoveIt (home, vertical, zero), so they might not work. Finally, it creates the ros publisher that publishes the arm's feedback on the base_feedback topic.
From what I see that is pretty much what KortexArmSimulation() does.

If your use case doesn't use these and you are able to not use the driver, I think you could get away without using it, but some features may be broken, I am not sure to want extent it is a necessity.

Out of curiosity, what did you remove exactly? What modifications did you do?

The next step to debug this issue would be to add ROS_INFO everywhere between lines 54 and 108 in kortex_arm_simulation.cpp and trying to find out what line is blocking the simulation driver initialization and why this happens.

@yj-Tang
Copy link

yj-Tang commented Jan 10, 2022

Actually, we didn't remove anything. We just combine the arm and our mobile base in one urdf file. Then create a moveit setup for the whole robot. The controllers can run correctly, but the driver of the arm always died.

We can control the arm after all the stuffs are loaded in Gazebo. However, the initial pose of the robot can not be controlled (maybe it is influenced by the death of the driver ) which is quite annoying.

I will try to debug with ROS_INFO then give the updates here.

@yj-Tang
Copy link

yj-Tang commented Jan 10, 2022

After adding the ROS_INFO, I think the error comes after the first loop of line 70. The outputs in terminals are:

[ INFO] [1641834504.399191400]: -------------------------------------------------
[ INFO] [1641834504.399563320]: line 56-robot_name 
[ INFO] [1641834504.399740919]: line 57-prefix
[ INFO] [1641834504.401829537]: line 62-robot_description
[ INFO] [1641834504.402037072]: line 63-robot_description
[ INFO] [1641834504.402875995]: line 67-arm_acceleration_max_limits
[ INFO] [1641834504.402892335]: line 69 completed
[ INFO] [1641834504.402905719]: line 70 completed, m_arm_joint_names : joint_1
[kinova_arm_driver-8] process has died [pid 353696, exit code -11, cmd /home/yujie/robot_pushing/pushing_training/noetic_husky_2/devel/lib/kortex_driver/kortex_arm_driver __name:=kinova_arm_driver __log:=/home/yujie/.ros/log/e9896e72-7237-11ec-8294-97f7c0019c07/kinova_arm_driver-8.log].
log file: /home/yujie/.ros/log/e9896e72-7237-11ec-8294-97f7c0019c07/kinova_arm_driver-8*.log
Topic [///joint_cmd] is not valid.
Service [///joint_cmd_req] is not valid.

Our robot namespace is "/", so maybe the error comes from something related to this topic name [///joint_cmd] ?

@felixmaisonneuve
Copy link
Contributor

So turns out I was wrong and the driver can find the model just fine. It is crashing when trying to get the arm joints at

auto joint = model.getJoint(m_arm_joint_names[i]);

You printed the value m_arm_joint_names[i] and, to me, joint_1 seems correct.

Can you copy the output of rosparam get /robot_description and rosparam get /kinova_arm_driver/joint_names?
If your Husky uses a joint named joint_1, the driver will find this joint instead of the arm's one. If that's the case, we will see it in the robot_description. You could use a prefix when launching your arm's driver. It will rename your joints to something like prefix_joint_1, it will solve the conflict (if that is the issue).

I am not sure what the ///joint_cmd topic and the ///joint_cmd_req service are. I have never seen these and I don't see any reference to them in ros_kortex.

@dfalveargOT
Copy link

Hi @yj-Tang, @civerachb-cpr, and @felixmaisonneuve, I have the same issue. Have you solved the problem?.
I'm trying to simulate Dingo-O with a gen-3 arm, then looking at the comments I followed all the suggestions without success.
Debugging the Kortex-arm-simulation I found:

  • The rosparam dump shows: is_initialized: false
  • Running roslaunch kortex_driver kortex_driver.launch shows Action client not connected: gen3_joint_trajectory_controller/follow_joint_trajectory.
  • I found that the code got stuck in m_follow_joint_trajectory_action_client->waitForServer();
    // Create and connect action clients
    m_follow_joint_trajectory_action_client.reset(new actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>(
    "/" + m_robot_name + "/" + m_prefix + m_arm_name + "_joint_trajectory_controller" + "/follow_joint_trajectory", true));
    m_follow_joint_trajectory_action_client->waitForServer();
  • Gazebo is not updating the joint's position.
    Thanks

@civerachb-cpr
Copy link
Author

@dfalveargOT unfortunately I haven't had much chance to dig deeper into this in a long time. Last time I tried I was still encountering these issues though, so it sounds like not much has changed between when I opened this ticket and now if you're still running into this.

@RX-00
Copy link

RX-00 commented Oct 6, 2022

@civerachb-cpr Hello! I'm also having the same problems detailed by everyone above. I haven't been able to find/create a fix either. Are there any updates/progress from the Clearpath side?

Thanks!

@civerachb-cpr
Copy link
Author

@RX-00, I've been swamped with other work lately and haven't had much time to revisit this. Last time I tried in May or June I was still having the same problems.

@civerachb-cpr
Copy link
Author

Sorry about the delay in replying. I'd forgotten about this ticket as I haven't had to do any work with arms in a long time.

I just tried it again; still seeing the same issues I previously reported. The situation appears unchanged since my last update six months ago.

@DavidPaulBenjamin
Copy link

We have this issue, too. We have a mobile robot with two gen3 arms. I changed line 60 in kortex_arm_simulation.cpp as you said, and I commented out the moveit startup lines 141 to 149. Now both arm drivers are running. They both say is_initialized is false. This is because kortex_arm_simulation.cpp has hung at line 192, which is:

m_follow_joint_trajectory_action_client->waitForServer

and never returns to kortex_arm_driver.cpp which sets is_initialized to true, as well as registering the simulation handlers to the simulation will run properly. Something appears to be wrong with the communication between the action clients and the action servers. Perhaps a namespace problem?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

6 participants