Issues with WholeBodyTrajectoryRosMessage in new API #44

mauricefallon · 2016-06-22T16:09:45Z

We've updated nearly all of our interface to the new API. Footstep plans and individual arm trajectories are fine as well as all the incoming sensor signals. (Yeah!)

But I'm having lots of issues with WholeBodyTrajectoryRosMessage (which we plan to use a lot). These issues are likely to be trival. I'm guessing the bits of information which make up the message are not being properly set:

ISSUE ONE
Its seems that I cannot provide a portion of WholeBodyTrajectoryRosMessage message e.g. arm/pelvis trajectories but not hand and foot poses (which would contridict). I was able to set execution_mode=QUEUE for the hand/feet trajs which forces the controller to ignore these parts of the message (it does print a warning). But this isnt what you intend, I think. I also explored changing the unique_id bit to 0 for these parts, which I think is not being checked for these unwanted components.

In the old API you could provide an empty list and the controller would ignore the components I don't want to provide.

ISSUE TWO
Pelvis tracking seems to be completely wrong for the WholeBodyTrajectoryRosMessage. It looks like the robot is shifting onto the left foot - which I didn't tell it to do. It looks like its standing on one foot. (Occasionally the controller crashed and mentioned the Flamingo stance)

So that you can easily reproduce, I've created a 50 line python script which will send a simple WholeBodyTrajectoryRosMessage. I expect this script will raise up the arms and shift the pelvis to the back & left by 6cm:
https://www.dropbox.com/sh/2ahizs03s8fwshl/AAD04x4huNFp4e9AWAeY9Yd5a?dl=0

But currently the pelvis is moving in the wrong direction so as to stand on one foot:
https://www.dropbox.com/s/zzwkmdro263kqon/simple_command_whole_body.m4v?dl=0

There are also simple pelvis and arm scripts which do work exactly as expected and in theory the WholeBodyTrajectoryRosMessage should just be a combination of them

We would really appreciate very simple python scripts to demo what should be the input for various messages. I remember such scripts from DRCSim being a huge time saver.

I see that you are busy with Unit A, and expect it to take time for you to test this.

dljsjr · 2016-06-22T16:22:28Z

In regards to your first issue, you should definitely be able to send whole body trajectories that only use parts of the body. The documentation for this message notes that the behavior of unique ID's in nested messages is different for Whole Body Trajectories than other messages where nested unique ID's are ignored:

Send whole body trajectories to the robot. A best effort is made to execute the trajectory
while balance is kept. A message with a unique id equals to 0 will be interpreted as invalid
and will not be processed by the controller. This rule DOES apply to the fields of this
message. If setting a field to null is not an option (going through IHMC ROS API), the user
can use the latter rule to select the messages to be processed by the controller.

Issue two is probably a real issue that we'll investigate on the robot. Thanks for providing the scripts.

mauricefallon · 2016-06-22T17:16:02Z

Issue one is a real issue:

When setting the left/right /hand/foot unique_id to zero:
msg.right_foot_trajectory_message.unique_id = 0

This causes the controller to crash:
msg.right_foot_trajectory_message.execution_mode = 0

While with this the controller doesn't crash:
msg.right_foot_trajectory_message.execution_mode = 1

You just need change the execution_mode bit in the python script and it will cause the controller to crash even through this part of the message is supposed to be ignored.

... that said this issue could be caused to Issue two

dljsjr · 2016-06-22T17:25:56Z

Is this issue specifically related to QUEUE mode? I'm a little vague on what the actual issue is. My understanding based on your most recent comment is:

If sending a whole body trajectory that only uses a subset of the messages AND setting a message to QUEUE, the controller will crash.

My original understanding of your bug was that it would reject the message outright. If so, then this is indeed a bug.

mauricefallon · 2016-06-22T18:05:20Z

No, what I see is that when I send a QUEUE message the controller refuses to execute it - which I think is what's expected. (I'm not actually interested in the QUEUE message at present)

But when I send an OVERRIDE message the controller crashes. Which leads me to believe that some part of this message is being attempted to be executed by the controller.... despite the unique_id of that component being 0.

All of this for hand/foot trajs within a nested Whole Body Trajectory.

dljsjr · 2016-06-22T19:30:39Z

Gotcha, I believe I understand now, thanks.

dljsjr · 2016-06-25T18:04:00Z

@mauricefallon Sylvain and I have taken a look at this and it seems that this bug has already been caught on develop. We were able to reproduce by using your script on the current beta tag, but when we run the commands on develop it seems to work. I have verified that develop on IHMCOpenRoboticsSoftware and develop on ihmc_ros_core does not currently have checksum mismatches on the ROS messages so you should be able to switch to develop without issue while you wait for us to tag the next beta release.

VladimirIvan · 2016-06-27T18:30:17Z

I can confirm that issues one and two are resolved in the new controller for simple trajectories generated in the python script.

However, I have following two issues with trajectories generated in our code:
ISSUE THREE - The chest orientation manager is failing with the following message:
Argument -0.005178999999999989 not in range [0.0, Infinity].

The message I sent worked with the previous API.

Is there a new joint limit for the back?

ISSUE FOUR - The pelvis trajectory manager fails with the following error:
Cannot exceed the maximum number of waypoints. Number of waypoints provided: 20

I have found the constant in the code. The limit is set to 15 waypoints in PelvisICPBasedTranslationManager.java at line 94.

Does the number of waypoints need to be limited?

I have bagged the command messages that generate these errors:
https://www.dropbox.com/sh/jcn1ovnncze1a5n/AADbVuUav_WwK28NnhfkL91sa?dl=0
All tested on the develop branch (8e0d2d4).

dljsjr · 2016-06-27T19:47:27Z

Glad we were able to fix the first two issues!

We're still trying to sort out our cross platform ROSBag workflow (#20) so would you mind providing a simple Python script similar to what Maurice provided? Additionally, copy/pasting the entire stack trace is also helpful.

Our suspicion is that you're providing a negative time, not negative joint limit, as that's the only place that looks for numbers in the range [0.0, Inf]. But there could be a bug in our code somewhere else.

SylvainBertrand · 2016-06-27T20:02:12Z

When possible, can you send us the stack trace of the error you're reporting, please. That'd be super helpful.
For the issue three, my guess is an issue related the waypoint time. I'm still looking for the exact cause.
For issue four, there's a way to remove the limitation on the number of waypoints (not really straightforward because of our data structure), I'll remove it ASAP for the pelvis.

VladimirIvan · 2016-06-28T12:16:39Z

I'll see if I can get reasonable python script to reproduce the issue.

The stacktrace of the range issue:

Exception while running simulation! Stack Trace:
java.lang.RuntimeException: java.util.concurrent.ExecutionException: java.lang.RuntimeException: java.lang.RuntimeException: Argument -0.005178999999999989 not in range [0.0, Infinity].
    at us.ihmc.simulationconstructionset.robotController.MultiThreadedRobotControllerExecutor.waitAndWriteData(MultiThreadedRobotControllerExecutor.java:78)
    at us.ihmc.simulationconstructionset.robotController.AbstractThreadedRobotController.doControl(AbstractThreadedRobotController.java:70)
    at us.ihmc.simulationconstructionset.Robot.doControllers(Robot.java:446)
    at us.ihmc.simulationconstructionset.Simulator.doControl(Simulator.java:126)
    at us.ihmc.simulationconstructionset.Simulator.simulate(Simulator.java:65)
    at us.ihmc.simulationconstructionset.Simulation.simulate(Simulation.java:392)
    at us.ihmc.simulationconstructionset.SimulationConstructionSet.simulateCycle(SimulationConstructionSet.java:2292)
    at us.ihmc.simulationconstructionset.SimulationConstructionSet.run(SimulationConstructionSet.java:2182)
    at java.lang.Thread.run(Thread.java:745)
Caused by: java.util.concurrent.ExecutionException: java.lang.RuntimeException: java.lang.RuntimeException: Argument -0.005178999999999989 not in range [0.0, Infinity].
    at java.util.concurrent.FutureTask.report(FutureTask.java:122)
    at java.util.concurrent.FutureTask.get(FutureTask.java:192)
    at us.ihmc.simulationconstructionset.robotController.MultiThreadedRobotControllerExecutor.waitAndWriteData(MultiThreadedRobotControllerExecutor.java:56)
    ... 8 more
Caused by: java.lang.RuntimeException: java.lang.RuntimeException: Argument -0.005178999999999989 not in range [0.0, Infinity].
    at us.ihmc.wholeBodyController.DRCControllerThread.run(DRCControllerThread.java:347)
    at java.util.concurrent.Executors$RunnableAdapter.call(Executors.java:511)
    at java.util.concurrent.FutureTask.run(FutureTask.java:266)
    at java.util.concurrent.ThreadPoolExecutor.runWorker(ThreadPoolExecutor.java:1142)
    at java.util.concurrent.ThreadPoolExecutor$Worker.run(ThreadPoolExecutor.java:617)
    ... 1 more
Caused by: java.lang.RuntimeException: Argument -0.005178999999999989 not in range [0.0, Infinity].
    at us.ihmc.robotics.MathTools.checkIfInRange(MathTools.java:475)
    at us.ihmc.robotics.math.trajectories.HermiteCurveBasedOrientationTrajectoryGenerator.setTrajectoryTime(HermiteCurveBasedOrientationTrajectoryGenerator.java:167)
    at us.ihmc.robotics.math.trajectories.HermiteCurveBasedOrientationTrajectoryGenerator.setTrajectoryParameters(HermiteCurveBasedOrientationTrajectoryGenerator.java:295)
    at us.ihmc.robotics.math.trajectories.waypoints.MultipleWaypointsOrientationTrajectoryGenerator.initializeSubTrajectory(MultipleWaypointsOrientationTrajectoryGenerator.java:223)
    at us.ihmc.robotics.math.trajectories.waypoints.MultipleWaypointsOrientationTrajectoryGenerator.compute(MultipleWaypointsOrientationTrajectoryGenerator.java:236)
    at us.ihmc.commonWalkingControlModules.controlModules.ChestOrientationManager.compute(ChestOrientationManager.java:117)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WalkingHighLevelHumanoidController.updateManagers(WalkingHighLevelHumanoidController.java:584)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WalkingHighLevelHumanoidController.doAction(WalkingHighLevelHumanoidController.java:513)
    at us.ihmc.robotics.stateMachines.GenericStateMachine.doAction(GenericStateMachine.java:114)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelHumanoidControllerManager.doControl(HighLevelHumanoidControllerManager.java:172)
    at us.ihmc.simulationconstructionset.robotController.ModularRobotController.doControl(ModularRobotController.java:20)
    at us.ihmc.wholeBodyController.DRCControllerThread.run(DRCControllerThread.java:339)
    ... 5 more

VladimirIvan · 2016-06-28T12:24:54Z

Do you have working rostopic command line tool?
If so, the following will reproduce the error:

rostopic pub /ihmc_ros/valkyrie/control/whole_body_trajectory ihmc_msgs/WholeBodyTrajectoryRosMessage -f topic.txt

You can download the topic data here.

dljsjr · 2016-06-28T12:56:42Z

I do, Sylvain doesn't as he's on Windows. I can play around with it though.

dljsjr · 2016-06-28T13:00:23Z

So without doing any topic playback and just looking at the file I don't see anywhere that you're setting a negative value for the time. I don't see the exact number from your exception either so it must be a computed value or something.

SylvainBertrand · 2016-06-28T13:53:45Z

The times for the chest are a mess:
t0 = 0.000,
t1 = 0.141,
t2 = 0.136,
t3 = 0.131 (and further it increases and decreases again).
The time has to be increasing from a waypoint to another.
I'll get the controller setup such that it'll let you know when that happens.

VladimirIvan · 2016-06-28T14:16:33Z

Well spotted. I fixed the timing and issue three went away.

I was also able to execute our plan by dropping the all the intermediate pelvis waypoints, reducing the pelvis trajectory to two waypoints. This validates that the message can be executed but it is not how we would like to command the robot. Such trajectory tracks the planned motion very poorly without the waypoints to constrain the pelvis and I can see the difference already in the simulator. In other words, issue four is still relevant to us.

SylvainBertrand · 2016-06-28T14:56:04Z

I hear you, I'll work on the pelvis issue today.
If you're working on the branch develop, I just pushed the fix that'll let you know when a message is bad.

SylvainBertrand · 2016-06-30T21:16:43Z

The issue related to the maximum number of waypoints should be fixed now. Let me know if you have any other issues with this message.

dljsjr · 2016-07-07T21:28:04Z

@VladimirIvan Now that you've upgraded to the new controller can you let us know if this is still an issue for you?

mauricefallon · 2016-07-18T12:59:11Z

We've been able to successfully execute whole body motion plans on the robot.

We may have some other issues when we look more closely at the execution, but all the issues mentioned here have been resolved

…e/refactor-point-cloud-in-NP to develop * commit '60da364bf72402605a236bde6d2e9a98e55d49c2': Added subscriber that can receive PointCloud2WithSource messages. Changed the lidar scan topic for Atlas. Fixed a stupid bug, was not setting the scan frame -_-'. Minor cleanup of the publisher.Some cleanup in the publisher. Updated the quadtree to the LidarScanPublisher. Added a getter that is much faster. Registered the new messages. Switched to the LidarScanPublisher. Unused class. Minor cleanup. Typos. Created LidarScanPublisher to replace the multitude of receivers.

dljsjr mentioned this issue Jun 26, 2016

Update and expand scripts for new ROS API ihmcrobotics/ihmc_ros_diagnostics#1

Closed

mauricefallon closed this as completed Jul 18, 2016

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Issues with WholeBodyTrajectoryRosMessage in new API #44

Issues with WholeBodyTrajectoryRosMessage in new API #44

mauricefallon commented Jun 22, 2016 •

edited

dljsjr commented Jun 22, 2016 •

edited

mauricefallon commented Jun 22, 2016

dljsjr commented Jun 22, 2016 •

edited

mauricefallon commented Jun 22, 2016

dljsjr commented Jun 22, 2016

dljsjr commented Jun 25, 2016

VladimirIvan commented Jun 27, 2016

dljsjr commented Jun 27, 2016 •

edited

SylvainBertrand commented Jun 27, 2016

VladimirIvan commented Jun 28, 2016

VladimirIvan commented Jun 28, 2016

dljsjr commented Jun 28, 2016

dljsjr commented Jun 28, 2016

SylvainBertrand commented Jun 28, 2016

VladimirIvan commented Jun 28, 2016

SylvainBertrand commented Jun 28, 2016

SylvainBertrand commented Jun 30, 2016

dljsjr commented Jul 7, 2016

mauricefallon commented Jul 18, 2016

Issues with WholeBodyTrajectoryRosMessage in new API #44

Issues with WholeBodyTrajectoryRosMessage in new API #44

Comments

mauricefallon commented Jun 22, 2016 • edited

dljsjr commented Jun 22, 2016 • edited

mauricefallon commented Jun 22, 2016

dljsjr commented Jun 22, 2016 • edited

mauricefallon commented Jun 22, 2016

dljsjr commented Jun 22, 2016

dljsjr commented Jun 25, 2016

VladimirIvan commented Jun 27, 2016

dljsjr commented Jun 27, 2016 • edited

SylvainBertrand commented Jun 27, 2016

VladimirIvan commented Jun 28, 2016

VladimirIvan commented Jun 28, 2016

dljsjr commented Jun 28, 2016

dljsjr commented Jun 28, 2016

SylvainBertrand commented Jun 28, 2016

VladimirIvan commented Jun 28, 2016

SylvainBertrand commented Jun 28, 2016

SylvainBertrand commented Jun 30, 2016

dljsjr commented Jul 7, 2016

mauricefallon commented Jul 18, 2016

mauricefallon commented Jun 22, 2016 •

edited

dljsjr commented Jun 22, 2016 •

edited

dljsjr commented Jun 22, 2016 •

edited

dljsjr commented Jun 27, 2016 •

edited