ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Moveit! for combining multiple robots

asked 2023-07-13 11:55:35 -0500

anirudh_chhabra gravatar image

updated 2023-07-13 18:39:02 -0500

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

ROS package: https://github.com/PI-PhysikInstrumen...

Platform 2: WidowX 250 Mobile Robot 6-DOF

Product Link: https://www.trossenrobotics.com/widow...

ROS Package: https://github.com/Interbotix/interbo...

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 ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2023-07-13 18:33:08 -0500

anirudh_chhabra gravatar image

updated 2023-07-15 10:42:20 -0500

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!

edit flag offensive delete link more

Comments

1

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

namespace perhaps?

gvdhoorn gravatar image gvdhoorn  ( 2023-07-14 03:37:56 -0500 )edit

oops. made a big typo there. I guess excitement wins over everything. Thanks. I will edit the answer.

anirudh_chhabra gravatar image anirudh_chhabra  ( 2023-07-15 10:42:03 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2023-07-13 11:55:35 -0500

Seen: 413 times

Last updated: Jul 15 '23