0
$\begingroup$

Rosanswers logo

Hello

I am trying to combine two robots using Moveit for combined motion planning. However, I am facing some issues. They could be very minor since I am new to this but would appreciate the help.

Following are some details.

Platforms and Libraries:

Platform 1: PI 6-DOF Hexapod H.811.i2

Product Link - https://www.pi-usa.us/en/products/6-axis-hexapods-parallel-positioners/h-811i2-6-axis-miniature-hexapod-700886

ROS package: https://github.com/PI-PhysikInstrumente/PI_ROS_Driver

Platform 2: WidowX 250 Mobile Robot 6-DOF

Product Link: https://www.trossenrobotics.com/widowx-250-mobile-robot-arm-6dof.aspx

ROS Package: https://github.com/Interbotix/interbotix_ros_manipulators/tree/main/interbotix_ros_xsarms

Problem Description:

I have placed the robotic arm on top of the hexapod and fixed it. To represent this, I combine the two urdf files and make a fixed joint between them. To achieve this, I use the xacro format.

Then, I use the MoveIt setup assistant to generate the MoveIt files. Everything works as expected during the simulation on rviz.

I move on to add the hardware interface nodes using the official packages and test the physical operation of the robot. To do that, my controllers.yaml looks like follows:

controller_list:
  - name: "joint_trajectory_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [cart_x, cart_y, cart_z, ang_u, ang_v, ang_w]
  - name: "mobile_wx250s/arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate]

As shown above, I load both the controllers for each robot. The hexapod publishes joint states as '/joint_states' and the robotic arm publishes as 'mobile_wx250s/joint_states'.

Therefore, I define the joint state publisher such that both the states are published to /tf.

Also, I go to move_group and remap 'mobile_wx250s/joint_states' to 'joint_states' so that planner knows all of them as a single body.

When I run the robots now, the /joint_states output is very weird - it fluctuates so that sometimes I don't receive any of the robotic arm states. The output of 'rostopic echo /joint_states/name' is as follows:

- cart_x
- cart_y
- cart_z
- ang_u
- ang_v
- ang_w
- axis0_base_anchor_joint_x
- axis0_base_anchor_joint_y
- axis0_base_anchor_joint_z
- axis1_base_anchor_joint_x
- axis1_base_anchor_joint_y
- axis1_base_anchor_joint_z
- axis2_base_anchor_joint_x
- axis2_base_anchor_joint_y
- axis2_base_anchor_joint_z
- axis3_base_anchor_joint_x
- axis3_base_anchor_joint_y
- axis3_base_anchor_joint_z
- axis4_base_anchor_joint_x
- axis4_base_anchor_joint_y
- axis4_base_anchor_joint_z
- axis5_base_anchor_joint_x
- axis5_base_anchor_joint_y
- axis5_base_anchor_joint_z
- axis0_platform_anchor_joint_x
- axis0_platform_anchor_joint_y
- axis0_platform_anchor_joint_z
- axis1_platform_anchor_joint_x
- axis1_platform_anchor_joint_y
- axis1_platform_anchor_joint_z
- axis2_platform_anchor_joint_x
- axis2_platform_anchor_joint_y
- axis2_platform_anchor_joint_z
- axis3_platform_anchor_joint_x
- axis3_platform_anchor_joint_y
- axis3_platform_anchor_joint_z
- axis4_platform_anchor_joint_x
- axis4_platform_anchor_joint_y
- axis4_platform_anchor_joint_z
- axis5_platform_anchor_joint_x
- axis5_platform_anchor_joint_y
- axis5_platform_anchor_joint_z
- waist
- shoulder
- elbow
- forearm_roll
- wrist_angle
- wrist_rotate

and sometimes as :

- ang_u
- ang_v
- ang_w
- axis0_base_anchor_joint_x
- axis0_base_anchor_joint_y
- axis0_base_anchor_joint_z
- axis0_platform_anchor_joint_x
- axis0_platform_anchor_joint_y
- axis0_platform_anchor_joint_z
- axis1_base_anchor_joint_x
- axis1_base_anchor_joint_y
- axis1_base_anchor_joint_z
- axis1_platform_anchor_joint_x
- axis1_platform_anchor_joint_y
- axis1_platform_anchor_joint_z
- axis2_base_anchor_joint_x
- axis2_base_anchor_joint_y
- axis2_base_anchor_joint_z
- axis2_platform_anchor_joint_x
- axis2_platform_anchor_joint_y
- axis2_platform_anchor_joint_z
- axis3_base_anchor_joint_x
- axis3_base_anchor_joint_y
- axis3_base_anchor_joint_z
- axis3_platform_anchor_joint_x
- axis3_platform_anchor_joint_y
- axis3_platform_anchor_joint_z
- axis4_base_anchor_joint_x
- axis4_base_anchor_joint_y
- axis4_base_anchor_joint_z
- axis4_platform_anchor_joint_x
- axis4_platform_anchor_joint_y
- axis4_platform_anchor_joint_z
- axis5_base_anchor_joint_x
- axis5_base_anchor_joint_y
- axis5_base_anchor_joint_z
- axis5_platform_anchor_joint_x
- axis5_platform_anchor_joint_y
- axis5_platform_anchor_joint_z
- cart_x
- cart_y
- cart_z

Also, on the main roslaunch terminal window, I see the following:

[ WARN] [1689202843.877877212]: The complete state of the robot is not yet known.  Missing waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate
[ WARN] [1689202843.885522041]: New joint state for joint 'ang_u' is not newer than the previous state. Assuming your rosbag looped.
[ WARN] [1689202843.929407546]: New joint state for joint 'cart_x' is not newer than the previous state. Assuming your rosbag looped.

This is causing some issues to use the Move Group Python API since the /joint_states is not complete all the time.

So my question is: WHAT AM I MISSING?

NOTE: I have created individual packages for both the robots using MoveIt while following the same process and they work like butter.

Here are some files probably needed to understand my moveit setup.

joint_names.yaml

controller_joint_names: [cart_x, cart_y, cart_z, ang_u, ang_v, ang_w, waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate]

my_planner.launch

<launch>
    
  <rosparam command="load" file="$(find my_pkg)/config/joint_names.yaml"/>

  <arg name="sim" default="true" />
  <arg name="robot_ip" unless="$(arg sim)" />

  <include file="$(find my_pkg)/launch/planning_context.launch" >
    <arg name="load_robot_description" value="true" />
  </include>

  <group if="$(arg sim)">
    <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  </group>

    <node name="pi_hardware_interface" pkg="pi_hexapod_control" type="pi_hexapod_control_node" output="screen" required="true">
      <rosparam command="load" file="$(arg hexapod_params)" />
      <param name="using_tcp_ip_connection" type="bool" value="$(arg using_tcp_ip_connection)" />
      <param name="hexapod_ip"              type="str"  value="$(arg hexapod_ip)" />
      <param name="hexapod_port"            type="int"  value="$(arg hexapod_port)" />
      <param name="using_rs232_connection"  type="bool" value="$(arg using_rs232_connection)" />
      <param name="rs232_port_nr"           type="int"  value="$(arg rs232_port_nr)" />
      <param name="baudrate"                type="int"  value="$(arg baudrate)" />
      <param name="is_sim"                  type="bool" value="$(arg sim)" />
      <param name="auto_referencing"        type="bool" value="$(arg auto_referencing)" />
      <param name="prefer_stop_over_halt"   type="bool" value="$(arg prefer_stop_over_halt)" />
    </node>

    <include file="$(find interbotix_xsarm_ros_control)/launch/xsarm_ros_control.launch">
      <arg name="robot_model"                       value="$(arg robot_model)"/>
      <arg name="robot_name"                        value="$(arg robot_name)"/>
      <arg name="base_link_frame"                   value="$(arg base_link_frame)"/>
      <arg name="show_ar_tag"                       value="$(arg show_ar_tag)"/>
      <arg name="use_world_frame"                   value="$(arg use_world_frame)"/>
      <arg name="external_urdf_loc"                 value="$(arg external_urdf_loc)"/>
      <arg name="use_rviz"                          value="false"/>
      <arg name="mode_configs"                      value="$(arg mode_configs)"/>
      <arg name="dof"                               value="$(arg dof)"/>
    </include>

    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <rosparam param="source_list">["/joint_states","/mobile_wx250s/joint_states"]</rosparam>
      <param name="rate" value="100"/>
      <param name="ignore_timestamp" value="true"/>
    </node>

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
      <param name="ignore_timestamp" value="true"/> -->
    </node>

    <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
      output="screen" args="$(arg controllers)">
    </node>

    <node name="ros_control_stopped_spawner" pkg="controller_manager" type="spawner" respawn="false"
      output="screen" args="--stopped $(arg stopped_controllers)">
    </node>
  </group>

  <include file="$(find my_pkg)/launch/move_group.launch">
    <arg name="publish_monitored_planning_scene" value="true" />
  </include>

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />
  <include file="$(find my_pkg)/launch/moveit_rviz.launch">
    <!-- <arg name="config" value="true"/> -->
    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

</launch>

Originally posted by anirudh_chhabra on ROS Answers with karma: 16 on 2023-07-13

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Solved it!

I realized the issue using an rqt graph.

The problem:

  1. The hexapod was publishing its joint states to /joint_states
  2. Robotic arm was publishing to /mobile_wx250s/joint_states
  3. Then the joint_state_publisher was combining both of them using <rosparam param="source_list">["/joint_states","mobile_wx250s/joint_states"]</rosparam> and sending them off back to /joint_states
  4. Therefore, there were two set of joint states being published to /joint_states and obviously this is why sometimes I would see the states of only hexapod and sometimes of both robots.

All I had to do was move the hexapod nodes into their own namespace using <group ns="pi_robot"> and then combine them using the joint_state_publisher using <rosparam param="source_list">["pi_robot/joint_states","mobile_wx250s/joint_states"]</rosparam>.

This fixed the issue and then an independent robot_state_publisher was properly publishing the /tf as well.

Seems like a rookie mistake but using the rqt_graph helped me a lot.

Hope this is helpful to someone.

Cheers!


Originally posted by anirudh_chhabra with karma: 16 on 2023-07-13

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by gvdhoorn on 2023-07-14:\

All I had to do was move the hexapod nodes into their own workspace [..]

namespace perhaps?

Comment by anirudh_chhabra on 2023-07-15:
oops. made a big typo there. I guess excitement wins over everything. Thanks. I will edit the answer.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.