motoman alarm 4414 after stopping trajectory and restart

asked 2019-05-01 07:03:31 -0500

Harry XS gravatar image

Hello at all and a nice may!

I'm working on a motoman ES200rdii with a DX200 controller with melodic. Everything is working nice and smooth. (If everything is ok I will post my URDF of this robot for the motoman_experimental package) Planning with MoveIt via moveit_commander works like expected.

But I have one strange thing happen:

I'm using a laser sensor that measures the distance between my picker-tool and the object. If the distance is smaller than 50mm I want to stop the movement, measure the position from the start point and drive to another goal. What happens now is: I plan a path from my startposition to my startposition.z += -0.58

            pose_target = robby.group.get_current_pose().pose
            pose_target.position.x += 0
            pose_target.position.y += 0
            pose_target.position.z += -0.58
            robby.group.set_pose_target(pose_target)
            plan = robby.group.plan()
            robby.group.execute(plan, wait=False)

This is working as expected. Then I want to stop this movement when the laser_1 sensor fires:

            _to = time.time()
            while psensors.laser_1 == False and time.time()-_to < 20:
                pass

            robby.group.stop()
            robby.group.clear_pose_targets()

This is working too. But now I want to drive to the startpostion again and at this moment the DX200 goes into an alarm 4414 telling me that S(L)UR(B)T Joints are overloaded, moved to fast. This is absolutely strange since I already drive with only 10%:

 self.group.set_max_velocity_scaling_factor(0.1)
 self.group.set_max_acceleration_scaling_factor(0.2)

and totally strange is that when I stop my node, stop MoveIt, restart everything I can easily drive from that position to the startpoint, also in Highspeed.

I thought maybe I have some trajectory points left that the controller wants to drive through, but I thought that they will be cleared with

robby.group.clear_pose_targets()

I already tried with the cartesian planner and the direct controller stop service (stop_motion).

Thank you in advance for your help, I'm totally stucked.

edit retag flag offensive close merge delete

Comments

This sounds like there is a desync between what the controller is doing (ie: has as outstanding trajectory pts) and what MoveIt is sending it, but without running into the "start state != current state" somehow.

Can you capture the network traffic between your ROS PC and the controller using Wireshark and make that available here?

gvdhoorn gravatar image gvdhoorn  ( 2019-05-01 08:06:42 -0500 )edit
gvdhoorn gravatar image gvdhoorn  ( 2019-05-01 08:11:15 -0500 )edit

And some initial feedback:

I thought maybe I have some trajectory points left that the controller wants to drive through, but I thought that they will be cleared with

robby.group.clear_pose_targets()

This only clears a MoveIt internal data structure. It does not interact with the driver at all.

gvdhoorn gravatar image gvdhoorn  ( 2019-05-01 09:53:14 -0500 )edit

Thanks. You can find the wireshark capture file under: link text

Hopefully I got it right. The DX200 controller is the 192.168.255.1 The Host is 192.168.255.11

I started a linear movement and send a group.stop() after 2 seconds. After 1 more second I started another movement and the alarm 4414 immediatly occured.

Harry XS gravatar image Harry XS  ( 2019-05-03 03:42:50 -0500 )edit

According to the code, upon receiving a STOP_MOTION command, MotoROS clears the queue (locally on the controller, here).

From the wireshark capture it would appear the message exchange is OK. We see the STOP_MOTION and the acknowledgement. The next trajectory does appear to start at the then "current state" of the robot (there is a very minor deviation (ie: at the 5th decimal after the comma) in J4, but I doubt that could be causing this).

I would suggest to post an issue on the motoman_driverissue tracker so we can further investigate this. Please mention this post on ROS Answers and any other pertinent information when you post your issue (ie: version of the controller system software, version of MotoROS, version of motoman_driver, etc).

gvdhoorn gravatar image gvdhoorn  ( 2019-05-03 04:39:52 -0500 )edit

What would be a good check would be to write a simple Python script where you just use the FollowJointTrajectory action server directly. Setup a goal, execute it, don't wait on the result, cancel it after some time, wait for the robot to settle, retrieve the current joint states, start a new trajectory starting from those current joint states.

If that also exhibits this problem, then we have at least excluded MoveIt.


Edit: this script could serve as an example.

gvdhoorn gravatar image gvdhoorn  ( 2019-05-03 04:41:45 -0500 )edit

Thank you for diving into this and sorry for the delay in my answer. I'll do this on monday evening, since I'm not at the lab. I'll first check the versions, then try it with talking to the action server directly. If the problem still persists I will post it on the motoman_driver issue tracker. If not, I'll post it here.

Harry XS gravatar image Harry XS  ( 2019-05-05 11:56:26 -0500 )edit

If I only use the action server directly still the same problem occurs. But if I use /robot_disable after the STOP_MOTION command and use robot_enable before the next command it works. I read that I should do this: b. After the motion is complete, call the "robot_disable". This stops the job when the Motoplus is in the IDLE state.

Is this the correct way to go or is it still something for the bug tracker?

Harry XS gravatar image Harry XS  ( 2019-05-06 15:01:21 -0500 )edit