Dual Arm Operation Failure

[josephbhlee]

Each Part(Left Arm, Right Arm and Toroso) is operated well, but I have failed to operate dual arms(left and right arm) at the same time.
I share my application source code as follows.

actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>* t_client;

std::vector ppoints[2];
std::vector pvelocities[2];

//Use current pose as first trajectory point
ppoints[0] = readJointAngles();
pvelocities[0] = readJointVelocities();

//Use goal for position and 0 for velocity of second trajectory point
ppoints[1].assign(goal,goal+15);
pvelocities[1].assign(15,0.0);
double d_msg[2] = {0,duration};

control_msgs::FollowJointTrajectoryGoal msg = create_goal_message(ppoints, pvelocities,2,d_msg);
 t->client->sendGoal(msg);

Error messages are
-. Invalid number of joint
-. Invalid trajectory. Not sent

I don't know where my problems are.

Could you help me to solve this problem?

[josephbhlee]

When I used the latest version of "motoman_sda10f_moveit_config", I could operate left arm and right arm well.
However, I faced the error message when I try to operate dual arms as follows.

[ INFO] [1647286841.972743989]: Execution request received
motoman_driver_joint_trajectory_action: /usr/include/boost/smart_ptr/shared_ptr.hpp:734: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = const control_msgs::FollowJointTrajectoryFeedback_<std::allocator<void> >; typename boost::detail::sp_member_access<T>::type = const control_msgs::FollowJointTrajectoryFeedback_<std::allocator<void> >*]: Assertion `px != 0' failed.
[joint_trajectory_action-5] process has died [pid 846046, exit code -6, cmd /home/jose-admin/ros_ws/devel/lib/motoman_driver/motoman_driver_joint_trajectory_action __name:=joint_trajectory_action __log:=/home/jose-admin/.ros/log/13244550-a3ce-11ec-bba8-6db9ab84267f/joint_trajectory_action-5.log].

When I check the controller.yalm , it already consider dual arms environment.

[josephbhlee]

I use the following commands.

$roslaunch  motoman_sda10f_moveit_config moveit_planning_execution.launch controller:=fs100 robot_ip:=10.0.0.2

[marip8]

FYI, the controller.yaml file that you linked to is a MoveIt-specific file that defines the motion groups for planning. The control groups that Motoman controller knows how to move are defined in the motion_interface.yaml file in the support package. For the SDA10F, there are four motion groups: one for each arm and one for each torso.

Your issue seems to be similiar to #251. I haven't used an SDA10F before, but you may actually have to command all four motion groups (both arms and both torsos) together to get the behavior you are looking for. I seem to remember that the controller can only move one motion group at a time or all four motion groups together, not any arbitrary combination of groups. That might be documented in #251 or @ted-miller and @gavanderhoorn can fact-check me on that. If you go the route of moving all four groups, the torso joints could just remain fixed at their initial positions.

I'm also not sure how you are creating the joint trajectory goal, but it also needs to include the names of all the joints that you are commanding.

[ted-miller]

You are correct that all four groups must be commanded together. Additionally, the third and fourth groups are physically the same axis (torso). So, these must be commanded with the same values.

[josephbhlee]

When the commands(TORSO, Left Arm and Right Arm) are sent at the same time, the dual arm does not work well. Usually system waits for the motion to finish before attempting to move the next arm.
I programmed as follows.

 trj_client_base->sendGoal(msg_base);
  //status = readTrajectoryStatus(TORSO);
  trj_client_left->sendGoal(msg_left);
  //status = readTrajectoryStatus(LEFT_ARM);
  trj_client_right->sendGoal(msg_right);
  //status = readTrajectoryStatus(RIGHT_ARM);

Secondly, I try to operate dual arms with the following code.
However it is not dual arms motion because the system sends position to each arm one at a time and waits for the motion to finish before continuing with the next arm.

ros::NodeHandle nh;
  yaskawa robot(nh);
  .........................................
  robot.sendJointGoal(point,duration,LEFT_ARM);
  while(robot.ok()){
    robot.update();
    status = robot.readTrajectoryStatus(LEFT_ARM);
    if(status.compare("SUCCEEDED")==0){
      break;
    }
    rate.sleep();
  }

  robot.sendJointGoal(point,duration,RIGHT_ARM);
  while(robot.ok()){
    robot.update();
    status = robot.readTrajectoryStatus(RIGHT_ARM);
    if(status.compare("SUCCEEDED")==0){
      break;
    }
    rate.sleep();
  }
 
  robot.sendJointGoal(point_b,duration,TORSO);
  while(robot.ok()){
    robot.update();
    status = robot.readTrajectoryStatus(TORSO);
    if(status.compare("SUCCEEDED")==0){
      break;
    }
    rate.sleep();
  }

Any mistakes in my codes to operate dual arms at the same time?

[gavanderhoorn]

@josephbhlee: for SDA robots, there are a few things you need to take into account.

First: make sure to use #259, instead of main-line kinetic-devel. I'd suggest rebasing the changes from #259 on-top of kinetic-devel though, as otherwise you'll miss out on quite a few fixes applied since that time.

Also, as already mentioned, the SDAs require the two joints in the torso to always be sent to the exact same coordinates.

With #259, you should be able to send goals to individual groups (ie: arms) by submitting them to the per-group joint_trajectory_action servers, or for all groups by sending them to the global joint_trajectory_action server.

But this is either-or: you cannot send individual goals to the left and the right arm at the same time, that's not supported.

If you need multiple groups to move at the same time, you have to send a single goal, with information for all those groups, to the global joint_trajectory_action server.

---

Edit: I realise you're using C++, but just to make sure we're using the same approach, please take a look at kinetic-devel...gavanderhoorn:actionclient_example, which shows an example of an action client script. That's for a robot with a single motion group, but if you include values for all joints in your SDA, it should work, as it's using the global joint_trajectory_action server topic.

Do make sure to be using #259 though.

[mosernick]

Thank you @gavanderhoorn, @ted-miller, and @marip8 for your help. I have been working with Joseph to try to fix this issue. Multi-group motion was still failing even after following your suggestions and we kept getting an "Invalid Robot ID" error. After some digging, we realized that the problem was that the controller did NOT actually have the fourth group (torso B2) connected as you can see in the picture. Once we realized this, we simply modified the yaml files to include only three groups and now multi-group motion works without a hitch.

I don't know how this happened but my question now is, do you foresee this causing any other issues further down the road? I would just as soon keep it as is, since it makes things much simpler and I'm not sure why there are two groups for the single torso joint in the first place. Is this really something that is worth fixing?

[ted-miller]

The only potential issue that I can foresee is if you transition back to using Inform jobs instead of ROS. There may be an issue if you attempt to program an R2-only job and try to coordinate it with the torso. But, if you use an R1+R2 job, then you should be fine.