Demo stickbot_humanoid_32dof does not stabilize about controlit HOT 13 CLOSED

I believe the problem is the controller takes a few seconds to start sending commands after Gazebo is started resulting in the robot falling too far to be recoverable.

from controlit.

have you ever tried to reset the initial position at other stand posture? I meet the same problem, is there any possible that some certain position trigger the collapse.

from controlit.

Yes I have tried other initial configurations mostly decreasing the initial angle of the joints. Fundamentally the center of pressures need to reside away from edge of feet and the center of mass must remain above convex polygon surrounding the feet.

from controlit.

I instrumented Coordinator and SMControlPlugin with print statements indicating when the first state is transmitted to ControlIt! and the first command is issued by ControlIt!.

Here are the commits:

• dbf6115
• liangfok/controlit_configs@cdd83f0

The results using stickbot_lowerleg_3dof are as follows:

• first state transmission: 06-07-2015 15:58:14.518232
• first command transmission: 06-07-2015 15:58:14.729794

They show that about 0.211562 seconds (211.562 ms) pass between the start of the simulation and the time the first command arrives. I believe this long duration is the source of the problem.

from controlit.

I first instrumented Coordinator to print the time the state is received for the first time. Here is the commit:

• 2bb8a72

Here is the result using stickbot_lowerleg_3dof:

• first state transmission: 06-07-2015 16:18:50.606306
• first state reception: 06-07-2015 16:18:50.816704
• first command transmission: 06-07-2015 16:18:50.820873

The time between sending and receiving the first state is 0.210398 seconds (210.398 ms). The time between receiving the first state and sending the first command is 0.004169 seconds (4ms). I believe we need to significantly reduce the latency of initial state transmission to prevent the robot from falling too far to recover.

from controlit.

Prior to starting Gazebo, ControlIt! states that it cannot connect to /smi/rtt_rx, /smi/joint_states, and /smi/gazebo/root_link_odom, but will try again later.

SMControlPlugin (27858), Load: Setting up /joint_states publisher
[ INFO] [1433753988.825670915]: SharedMemoryTransport (27910): Configured transport for smi:rtt_tx.
[ INFO] [1433753988.825804599]: SharedMemoryTransport (27910): Connected to smi:rtt_tx.
[ INFO] [1433753988.827060154]: SharedMemoryTransport (27910): Configuring smi:rtt_rx transport.
[ INFO] [1433753988.827154918]: SharedMemoryTransport (27910): Configured transport for smi:rtt_rx.
[ WARN] [1433753988.828201109]: /stickbot_lowerleg_3dof_controller/controlit_exec: Couldn't connect to /smi/rtt_rx via shared memory! Will try again later! Returning false for now.
[ INFO] [1433753988.832085753]: SharedMemoryTransport (27910): Configuring smi:joint_states transport.
[ INFO] [1433753988.832236101]: SharedMemoryTransport (27910): Configured transport for smi:joint_states.
[ WARN] [1433753988.833286768]: /stickbot_lowerleg_3dof_controller/controlit_exec: Couldn't connect to /smi/joint_states via shared memory! Will try again later! Returning false for now.
[ INFO] [1433753988.838361376]: SharedMemoryTransport (27910): Configuring smi:gazebo-root_link_odom transport.
[ INFO] [1433753988.838659489]: SharedMemoryTransport (27910): Configured transport for smi:gazebo-root_link_odom.
[ WARN] [1433753988.839774666]: /stickbot_lowerleg_3dof_controller/controlit_exec: Couldn't connect to /smi/gazebo/root_link_odom via shared memory! Will try again later! Returning false for now.
[ INFO] [1433753988.909610043]:  [03:59:48] [package: controlit_core] [file: Coordinator.cpp] [function: start] [line: 331] [pid: 27910]: Starting whole body controller...

SMControlPlugin (27858), Load: Setting up /rtt_rx publisher
SMControlPlugin (27858), Load: Setting up /cmd subscriber
SMControlPlugin::Load: Publishing to odometry topic /gazebo/root_link_odom
SMControlPlugin::Load: Done loading plugin.

The connections are established after Gazebo is started. This is why it takes so long for ControlIt! to receive the first state message from Gazebo.

SMControlPlugin (28673): Sending first state at time:06-08-2015  04:03:27.652254
[ INFO] [1433754207.757716007, 0.101000000]: SharedMemoryTransport (28725): Connected to smi:gazebo-root_link_odom.
[ INFO] [1433754207.758063016, 0.101000000]: SharedMemoryTransport (28725): Connected to smi:joint_states.
[ INFO] [1433754207.759208747, 0.103000000]: SharedMemoryTransport (28725): Connected to smi:rtt_rx.
[ INFO] [1433754207.859821330, 0.201000000]:  [04:03:27] [package: controlit_core] [file: Coordinator.cpp] [function: servoUpdate] [line: 393] [pid: 28725]: Receiving first state at time 06-08-2015  04:03:27.859720

[ INFO] [1433754207.863057279, 0.204000000]:  [04:03:27] [package: controlit_core] [file: Coordinator.cpp] [function: servoUpdate] [line: 516] [pid: 28725]: Sending first command at time 06-08-2015  04:03:27.862987

I modify SMControlPlugin and RobotInterfaceSM to publish one message on each shared memory topic and to wait for the message to be received during initialization.
Just prior to starting simulator:

SMControlPlugin (10167), Load: Setting up /joint_states publisher
SMControlPlugin (10167), Load: Setting up /rtt_rx publisher
SMControlPlugin (10167), Load: Setting up /cmd subscriber
[ INFO] [1433756473.102230521]: SharedMemoryTransport (10218): Connected to smi:rtt_rx.
[ INFO] [1433756473.121063803]: SharedMemoryTransport (10218): Connected to smi:joint_states.
SMControlPlugin::Load: Publishing to odometry topic /gazebo/root_link_odom
SMControlPlugin (10167), Load: Waiting for messages...
[ INFO] [1433756473.351533874]: SharedMemoryTransport (10218): Configuring smi:gazebo-root_link_odom transport.
[ INFO] [1433756473.351995361]: SharedMemoryTransport (10218): Configured transport for smi:gazebo-root_link_odom.
[ INFO] [1433756473.352328554]: SharedMemoryTransport (10218): Connected to smi:gazebo-root_link_odom.
[ INFO] [1433756473.358072752]:  [04:41:13] [package: controlit_robot_interface_library] [file: RobotInterfaceSM.cpp] [function: init] [line: 163] [pid: 10218]: Publishing dummy cmd and RTT TX messages...

SMControlPlugin (10167), Load: Initial command message received.
SMControlPlugin (10167), Load: Initial RTT TX message received.
SMControlPlugin::Load: Done loading plugin.
[ INFO] [1433756473.420034758]:  [04:41:13] [package: controlit_core] [file: Coordinator.cpp] [function: start] [line: 331] [pid: 10218]: Starting whole body controller...

After starting simulator:

SMControlPlugin (10167): Sending first state at time:06-08-2015  04:42:40.596557
[ INFO] [1433756560.712368242, 0.104000000]:  [04:42:40] [package: controlit_core] [file: Coordinator.cpp] [function: servoUpdate] [line: 393] [pid: 10218]: Receiving first state at time 06-08-2015  04:42:40.712218

[ INFO] [1433756560.715356845, 0.104000000]:  [04:42:40] [package: controlit_core] [file: Coordinator.cpp] [function: servoUpdate] [line: 516] [pid: 10218]: Sending first command at time 06-08-2015  04:42:40.715266

By ensuring the shared memory connections are created prior to starting Gazebo, it now takes 0.115661 seconds (115.661 ms) to receive first state from Gazebo. This is unfortunately still too long and the robot goes unstable shortly after starting the simulation.

from controlit.

Gazebo sends Controlit! three messages:

1. nav_msgs::Odometry - for odometry data
2. std_msgs::Int64 - for reflected RTT message
3. std_msgs::Float64MultiArray - for joint state data

ControlIt! sends Gazebo two messages:

1. std_msgs::Float64MultiArray - command
2. std_msgs::Int64 - RTTmessage

When the simulation starts, the odometry messages are received right away. This is because the subscriptions to /joint_states and /rtt_rx are initially stuck in calls to smt->connect(). Connections to /joint_states and /rtt_rx failed when initially attempted. Apparently this initial failure results in a long 100+ ms delay.

from controlit.

Since I'm unable to debug the high initial latency problem, I wrote a unit test that demonstrates the problem. It is documented here: liangfok/ros_shared_memory_interface#1

from controlit.

The initial latency problem was caused by shared_memory_interface::Subscriber::callbackThreadFunction(...) using ros::Rate to loop when trying to connect to the shared memory topic. Since Gazebo is initially paused, the simulated clock is not advancing. This prevented ControlIt! from connecting to the shared memory topic prior to the start of the simulation. By pre-connecting, the initial communication latency after the simulation begins is reduced to less than 1ms.

The fix was to replace the use of ros::Rate with usleep(...). Here is the commit: liangfok/ros_shared_memory_interface@7614cb6

from controlit.

By reducing the initial communication latency, I was able to increase the damping gains for the torso pitch joints. This eliminated the whole body oscillations that were causing the robot's feet to slip. Here is the commit: liangfok/controlit_configs@aa5ac34

The joint position control demo is now stable.

from controlit.

The COM demo works with minor adjustments. Here is the commit: liangfok/controlit_configs@ba0f6bd

from controlit.

To further improve startup reliability, I modified the Gazebo shared memory plugin to wait for the first command to arrive before issuing it to the simulated robot. Here is the commit: liangfok/controlit_configs@40b9de7

from controlit.

Probably the biggest impact on stability is the use of build type "Release" instead of "RelWithDebInfo". The switch was made in commit: f380ce9

With this change, the servo compute latency dropped from over 20ms to less than 1ms. All three stickbot_humanoid_32dof_controlit demos now work.

from controlit.