Robot shaking when sending both arm trajectories at 100Hz

[v-lopez]

We are using the latest version released for the srcsim competition.

Steps to reproduce issue

We are sending the commands in the attached rosbag, in them we control each arm at 100Hz. So we're publishing alternating messages in the /ihmc_ros/valkyrie/control/arm_trajectory topic, one for left, one for right, each one with increasing unique_id's.

Expected behavior vs Actual behavior

Controling like described above, causes the robot to have small spasms in which all the body jerks a little bit and makes manipulation impossible. If we don't publish one of the arms, the robot moves smoothly.

Are we doing something wrong?

Thanks for the help.

robot_spasms.zip

[dljsjr]

Hey @v-lopez,

You do not need to stream commands to the controller. Once a trajectory has been executed the controller will hold the arm in whatever frame you told it to use for control until another command comes in.

High frequency input to the API is basically telling the controller to constantly make a bunch of micro-corrections.

[v-lopez]

We are not sending the same command at every message, we are using our own control framework to generate the trajectory online and streaming it's output to the ihmc controller.

There's a significant difference when sending commands for one or two arms, with one it works smoothly.

[v-lopez]

You can see in this video the spasms I mention. This was recorded when controlled with two arms, as you can see in the rosbag, our control is very unaggressive and doesn't include those hicups the robot does. The further away from the body the arm is, the more significant the hiccups are.

[dljsjr]

Awesome, thanks for the video and the clarification. I figured there was more going on which is why I left the issue open.

For things related to the internals of the controller typically a rosbag isn't quite enough for us, we're in the process of working with OSRF to get a new release out that allows for logging of Gazebo simulations using our logging server, it should be available pretty soon.

I'll do what I can with the rosbag in the meantime but I suspect we'll need the additional data from the controller logs.

[dljsjr]

@v-lopez I'm not currently in front of a ROS machine to do the bag playback and it might be a day or two until I can, so just a quick question: Are you using OVERRIDE or QUEUE mode for the trajectories you're emitting?

[v-lopez]

We are using Override.

[nmertins]

@v-lopez In the rosbag you uploaded, it looks like you only included a trajectory for the right arm. Was that intentional? There's also chest, neck and pelvis trajectories. If I only playback the arm trajectory, everything seems fine.

[dljsjr]

@v-lopez In addition to @nmertins question, I'm also curious as to what would happen if you used QUEUE mode.

[v-lopez]

For queue mode see #14, I can't seem to play any motion with it. At one point I didn't have that exception but I got errors about full buffers, I decresed the frequency to 25Hz but faced the above exception again and I desisted.

For OVERRIDE I have recorded two new bags, both trying to reach the same point that is above and past the valve. See these videos: Both Arms Shaking Only Left Smooth

both arms & only left bags

And here I have plotted the trajectory for one of the joints of the arms, you can see that the trajectory is very smooth and similar for both executions.

[v-lopez]

I tested today sending both arms at 50Hz each, in case the problem was going at 200 Hz. But no, I have the same issue commanding both arms at 50Hz.

100Hz just left arm remains stable.

[dljsjr]

@v-lopez Thanks for the update.

If you continue to experiment with this then please continue to provide more data and rosbags, both myself and @nmertins will be watching this issue. But realistically we probably can't really do much with this until we get the controller logging released and hand it off to the controls folks. We're still working with OSRF on getting the release out to everyone.

Just to follow on with my previous comments about not needing to stream, the API was originally designed to be an endpoint that would receive commands from things like a human operator or non-real-time/asynchronous planners. Anything faster than a few Hz has simply never been tested by us (other folks might be doing the same as you are but we don't have visibility in to that). So we personally don't have much of a frame of reference here. In theory there should be absolutely no issue with processing commands at this rate but in practice we've just never subjected the trajectory processing code to that sort of input.

I'll let you know as soon as OSRF makes the release with logging enabled and we can go further, but again if you continue to find new insights please continue to share them so we can try to get this resolved for you.

[v-lopez]

Yes we are continuously trying to find out any other hints on our side of what might be wrong.

Right now this is limiting blocking for us because we need to plan all the tasks to use just one arm.

Looking forward to getting the debugging software, any ETA? There's 5 weeks until the finals dry run and we can't postpone much further the implementation.

[dljsjr]

The code has been pushed up on our end, we're just waiting for OSRF to vet and make a release.

My suggestion at this point is to convert your planner to create a single large trajectory message instead of hundreds of individual messages and see if that fixes anything. I make no promises that it will.

[v-lopez]

Great news, paging @scpeters to see if this can get a little push for the SRC finals.

[v-lopez]

Using the WholeBodyTrajectoryRosMessage seems to be fine in the sense that it doesn't shake.

But we cannot use it, because we are only interested in moving the arms, neck, pelvis orientation and pelvis height, not the whole pelvis trajectory required by that message.

Would it be possible to have a topic "/ihmcros/valkyrie/control/two arm_trajectory" or something similar where both arms are published in a single message? In the same way as the WholeBodyTrajectoryRosMessage.

[dljsjr]

@v-lopez WholeBodyTrajectoryMessages can be partial. From the documentation at the top of the .msg file, this is one of the only messages where the Unique ID of the message fields is processed instead of only processing the Unique ID of the container message.

So to only send arm commands, you would set all of the other trajectories to any value w/ a Unique ID of 0 and they'll be ignored.

[v-lopez]

Thanks @dljsjr, I didn't think I could use both APIs at the same time but the first tests look very promising.

I'm sending the arms via the WholeBody topic, and the rest on their respective topics, setting unique_id to 0 on the WholeBody topic.

I'll report back after a few days of testing, again thanks for the help!

[dljsjr]

Awesome, glad that seems to be working. Sorry you have to use such a convoluted work-around. Hopefully we can get the arm trajectory stuff resolved quickly once we have some log data.

[v-lopez]

Hi, sorry to hijack this issue but I do not want to open a new one.

We have seen that sometimes the controller stops abruptly when it detects some situations, our guess is that it is made that way to protect the hardware when used out of simulation.

Is there any topic we could monitor to detect how close is the controller from being stopped, a measure of instability or something that we can do, to detect we are doing something that is introducing errors on the controller?

Can the auto shutdown of the controller be disabled?

[calvertdw]

We have safeties to make sure the controller doesn't go crazy when it falls over. We normally have this disabled during simulation but I'm not sure if that's possible through the ROS API. We usually closely monitor the desired ICP vs the actual. The farther the desired gets from actual, the more likely the controller will shutdown before a fall.

[v-lopez]

Yes that's what we were monitoring. Good to know we are on the right track.

Thanks for the help.