search

robotology / walking-controllers

Bipedal locomotion software for the humanoid robot platform iCub.
BSD 3-Clause "New" or "Revised" License
125 stars 46 forks source link

iCubGazeboV2_5 robot falling before completing walking trajectory #28

Open prashanthr05 opened 5 years ago

prashanthr05 commented 5 years ago

@GiulioRomualdi @S-Dafarra

I have been testing the walking controller along side floating base estimation device. The robot tends to fall every time I run the WalkingModule and the module exits unanimously.

The walking parameters are default as the ones available in the repository, except for the following changes,

The steps followed to run the WalkingModule are as described in the README. I have been using setGoal 0.3 0.0 as input.

Background on floating base estimation device. This is a YARP device opening and attaching itself to the remote control boards of all parts of the iCubGazeboV2_5 robot. It also opens and attaches itself to the wholeBodyDynamics device. A transformServer is also opened in order to publish transforms to ROS \tf topic. The regular workflow involves also running a yarprobotstatepublisher along with rviz to visualize the estimation. All of this along with Gazebo is already a considerable load on the computer memory, causing things to slow down. In the involved components, rviz and Gazebo are quite heavy with respect to the PC's capability (8GB RAM 2.5GHz 4-core CPU)

I suspect the fact that both walking-controller and floating-base-estimation talks with whole-body-dynamics and this causes the walking-controller to not get the required Cartesian wrench feedback within the update period of the controller is causing the failure ? However, this is just my naive speculation.

I would ask about your opinion on why this failure could happen ? and precautions to avoid this failure.

traversaro commented 5 years ago

How are you synchronizing the Gazebo simulator and the walking controller? YARP_CLOCK=/clock ? Sorry, I did not read correctly. If you followed the README, indeed you are using YARP_CLOCK=/clock .

traversaro commented 5 years ago

If you do not run RViz and all the additional software, the walking simulation is working correctly?

S-Dafarra commented 5 years ago

Can you paste the last lines before the failure?

It is quite possible that when the robot fall, the module commits suicide due to a problem in the computation of the ZMP. But this is a consequence of the fall, not the cause. Since we are using the YARP_CLOCK it is possible to see difference in performance under heavy load, i.e. the robot may fall more often. Also we usually prefer to tune the behavior with the real robot, using the simulation as a proof of concept. This means that the performances of the controller in simulation may not be the best possible.

prashanthr05 commented 5 years ago

I was unable to run the WalkingModule because of the changes made in iDynTree OptimalControl resulting in the following leak,

YARP_CLOCK=/clock val rind WalkingModule kingModule 
==27481== Memcheck, a memory error detector
==27481== Copyright (C) 2002-2015, and GNU GPL'd, by Julian Seward et al.
==27481== Using Valgrind-3.11.0 and LibVEX; rerun with -h for copyright info
==27481== Command: WalkingModule
==27481== 
yarp: Port /iiticublap092/WalkingModule/27481/clock:i active at tcp://10.255.111.63:10159/
Success: port-to-port persistent connection added.
yarp: Waiting for clock server to start broadcasting data ...
yarp: Receiving input from /clock to /iiticublap092/WalkingModule/27481/clock:i using tcp
[INFO]The model is found in:  /home/pramadoss/iit_ws/robotology-superbuild/build/install/share/iCub/robots/iCubGazeboV2_5/model.urdf 
[WARNING]  :: createReducedModelAndSensors : The joint l_leg_ft_sensor is not in the reduced model, the associated joint sensor won't be present

[WARNING]  :: createReducedModelAndSensors : The joint r_leg_ft_sensor is not in the reduced model, the associated joint sensor won't be present

[WARNING]  :: createReducedModelAndSensors : The joint l_foot_ft_sensor is not in the reduced model, the associated joint sensor won't be present

[WARNING]  :: createReducedModelAndSensors : The joint r_foot_ft_sensor is not in the reduced model, the associated joint sensor won't be present

[WARNING]  :: createReducedModelAndSensors : The joint l_arm_ft_sensor is not in the reduced model, the associated joint sensor won't be present

[WARNING]  :: createReducedModelAndSensors : The joint r_arm_ft_sensor is not in the reduced model, the associated joint sensor won't be present

[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/torso/rpc:o active at tcp://10.255.111.63:10137/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/torso/command:o active at tcp://10.255.111.63:10138/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/torso/stateExt:i active at tcp://10.255.111.63:10139/
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/torso/rpc:o to /icubSim/torso/rpc:i using tcp
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/torso/command:o to /icubSim/torso/command:i using udp
yarp: Receiving input from /icubSim/torso/stateExt:o to /walking-coordinator/remoteControlBoard/icubSim/torso/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/left_arm/rpc:o active at tcp://10.255.111.63:10140/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/left_arm/command:o active at tcp://10.255.111.63:10141/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/left_arm/stateExt:i active at tcp://10.255.111.63:10142/
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/left_arm/rpc:o to /icubSim/left_arm/rpc:i using tcp
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/left_arm/command:o to /icubSim/left_arm/command:i using udp
yarp: Receiving input from /icubSim/left_arm/stateExt:o to /walking-coordinator/remoteControlBoard/icubSim/left_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/right_arm/rpc:o active at tcp://10.255.111.63:10143/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/right_arm/command:o active at tcp://10.255.111.63:10144/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/right_arm/stateExt:i active at tcp://10.255.111.63:10145/
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/right_arm/rpc:o to /icubSim/right_arm/rpc:i using tcp
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/right_arm/command:o to /icubSim/right_arm/command:i using udp
yarp: Receiving input from /icubSim/right_arm/stateExt:o to /walking-coordinator/remoteControlBoard/icubSim/right_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/left_leg/rpc:o active at tcp://10.255.111.63:10146/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/left_leg/command:o active at tcp://10.255.111.63:10147/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/left_leg/stateExt:i active at tcp://10.255.111.63:10148/
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/left_leg/rpc:o to /icubSim/left_leg/rpc:i using tcp
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/left_leg/command:o to /icubSim/left_leg/command:i using udp
yarp: Receiving input from /icubSim/left_leg/stateExt:o to /walking-coordinator/remoteControlBoard/icubSim/left_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/right_leg/rpc:o active at tcp://10.255.111.63:10149/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/right_leg/command:o active at tcp://10.255.111.63:10150/
yarp: Port /walking-coordinator/remoteControlBoard/icubSim/right_leg/stateExt:i active at tcp://10.255.111.63:10151/
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/right_leg/rpc:o to /icubSim/right_leg/rpc:i using tcp
yarp: Sending output from /walking-coordinator/remoteControlBoard/icubSim/right_leg/command:o to /icubSim/right_leg/command:i using udp
yarp: Receiving input from /icubSim/right_leg/stateExt:o to /walking-coordinator/remoteControlBoard/icubSim/right_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remotecontrolboardremapper>. See C++ class yarp::dev::RemoteControlBoardRemapper for documentation.
yarp: Port /walking-coordinator/leftFootWrench:i active at tcp://10.255.111.63:10152/
yarp: Receiving input from /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o to /walking-coordinator/leftFootWrench:i using tcp
yarp: Port /walking-coordinator/rightFootWrench:i active at tcp://10.255.111.63:10153/
yarp: Receiving input from /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o to /walking-coordinator/rightFootWrench:i using tcp
yarp: Port /walking-coordinator/rpc active at tcp://10.255.111.63:10154/
==27481== Jump to the invalid address stated on the next line
==27481==    at 0x0: ???
==27481==    by 0x5BF77A3: iDynTree::optimalcontrol::ConstraintsGroup::addConstraint(std::shared_ptr<iDynTree::optimalcontrol::Constraint>, iDynTree::optimalcontrol::TimeRange const&) (in /home/pramadoss/iit_ws/robotology-superbuild/build/install/lib/libidyntree-optimalcontrol.so)
==27481==    by 0x5BAD28A: iDynTree::optimalcontrol::OptimalControlProblem::addConstraint(std::shared_ptr<iDynTree::optimalcontrol::Constraint>) (in /home/pramadoss/iit_ws/robotology-superbuild/build/install/lib/libidyntree-optimalcontrol.so)
==27481==    by 0x4E53A0F: UnicycleOptimization::UnicycleOptimization() (in /home/pramadoss/iit_ws/loc2/unicycle-footstep-planner/install/lib/libUnicyclePlanner.so)
==27481==    by 0x4E4EB9A: UnicyclePlanner::UnicyclePlanner() (in /home/pramadoss/iit_ws/loc2/unicycle-footstep-planner/install/lib/libUnicyclePlanner.so)
==27481==    by 0x4E6DDBC: UnicycleTrajectoryGenerator::UnicycleTrajectoryGenerator() (in /home/pramadoss/iit_ws/loc2/unicycle-footstep-planner/install/lib/libUnicyclePlanner.so)
==27481==    by 0x467E94: TrajectoryGenerator::TrajectoryGenerator() (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481==    by 0x467FC5: _ZSt11make_uniqueI19TrajectoryGeneratorIEENSt9_MakeUniqIT_E15__single_objectEDpOT0_ (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481==    by 0x457999: WalkingModule::configure(yarp::os::ResourceFinder&) (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481==    by 0x761F251: yarp::os::RFModule::runModule(yarp::os::ResourceFinder&) (in /home/pramadoss/iit_ws/robotology-superbuild/build/install/lib/libYARP_OS.so.3.1.100)
==27481==    by 0x4141FE: main (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481==  Address 0x0 is not stack'd, malloc'd or (recently) free'd
==27481== 
==27481== 
==27481== Process terminating with default action of signal 11 (SIGSEGV)
==27481==  Bad permissions for mapped region at address 0x0
==27481==    at 0x0: ???
==27481==    by 0x5BF77A3: iDynTree::optimalcontrol::ConstraintsGroup::addConstraint(std::shared_ptr<iDynTree::optimalcontrol::Constraint>, iDynTree::optimalcontrol::TimeRange const&) (in /home/pramadoss/iit_ws/robotology-superbuild/build/install/lib/libidyntree-optimalcontrol.so)
==27481==    by 0x5BAD28A: iDynTree::optimalcontrol::OptimalControlProblem::addConstraint(std::shared_ptr<iDynTree::optimalcontrol::Constraint>) (in /home/pramadoss/iit_ws/robotology-superbuild/build/install/lib/libidyntree-optimalcontrol.so)
==27481==    by 0x4E53A0F: UnicycleOptimization::UnicycleOptimization() (in /home/pramadoss/iit_ws/loc2/unicycle-footstep-planner/install/lib/libUnicyclePlanner.so)
==27481==    by 0x4E4EB9A: UnicyclePlanner::UnicyclePlanner() (in /home/pramadoss/iit_ws/loc2/unicycle-footstep-planner/install/lib/libUnicyclePlanner.so)
==27481==    by 0x4E6DDBC: UnicycleTrajectoryGenerator::UnicycleTrajectoryGenerator() (in /home/pramadoss/iit_ws/loc2/unicycle-footstep-planner/install/lib/libUnicyclePlanner.so)
==27481==    by 0x467E94: TrajectoryGenerator::TrajectoryGenerator() (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481==    by 0x467FC5: _ZSt11make_uniqueI19TrajectoryGeneratorIEENSt9_MakeUniqIT_E15__single_objectEDpOT0_ (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481==    by 0x457999: WalkingModule::configure(yarp::os::ResourceFinder&) (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481==    by 0x761F251: yarp::os::RFModule::runModule(yarp::os::ResourceFinder&) (in /home/pramadoss/iit_ws/robotology-superbuild/build/install/lib/libYARP_OS.so.3.1.100)
==27481==    by 0x4141FE: main (in /home/pramadoss/iit_ws/robotology/walking-controllers/build/install/bin/WalkingModule)
==27481== 
==27481== HEAP SUMMARY:
==27481==     in use at exit: 3,792,243 bytes in 7,679 blocks
==27481==   total heap usage: 342,232 allocs, 334,553 frees, 36,572,135 bytes allocated
==27481== 
==27481== LEAK SUMMARY:
==27481==    definitely lost: 0 bytes in 0 blocks
==27481==    indirectly lost: 0 bytes in 0 blocks
==27481==      possibly lost: 14,672 bytes in 46 blocks
==27481==    still reachable: 3,777,571 bytes in 7,633 blocks
==27481==                       of which reachable via heuristic:
==27481==                         multipleinheritance: 14,688 bytes in 102 blocks
==27481==         suppressed: 0 bytes in 0 blocks
==27481== Rerun with --leak-check=full to see details of leaked memory
==27481== 
==27481== For counts of detected and suppressed errors, rerun with: -v
==27481== ERROR SUMMARY: 1 errors from 1 contexts (suppressed: 0 from 0)
Killed

So I what I did was to align this repository with the latest devel. However, in that case, I face an issue of UnicyclePlanner cmake. 

CMake Error at modules/Walking_module/CMakeLists.txt:19 (find_package):
  Could not find a configuration file for package "UnicyclePlanner" that is
  compatible with requested version "0.1.102".

  The following configuration files were considered but not accepted:

    /home/pramadoss/iit_ws/loc2/unicycle-footstep-planner/install/lib/cmake/UnicyclePlanner/UnicyclePlannerConfig.cmake, version: unknown

So I would like to confirm ? should I align UnicyclePlanner with its devel or stay in dcmGenerator branch ?

S-Dafarra commented 5 years ago

Actually you can put both walking-controllers and unicycle-planner in master.

prashanthr05 commented 5 years ago

@S-Dafarra

Can you paste the last lines before the failure?

[INFO]IK: 2.236400 ms Total: 3.628100 ms  
[INFO]IK: 2.411700 ms Total: 4.632300 ms  
[INFO]IK: 1.830900 ms Total: 2.829300 ms  
[ERROR][evaluateZMP] The total z-component of contact wrenches is too low. 
[ERROR][updateModule] Unable to evaluate the ZMP. 
[INFO]RFModule closing.

Actually, yes this seems to be the consequence and not the cause. The cause happens to be that the robot loses balance as it shifts all of its weight to one leg during swing phase.

Average load on the CPU is shown below, top

prashanthr05 commented 5 years ago

The 66% usage of CPU by yarprobotinterface is due to the components it is running, altogether with floating base estimation as described in the above comment

This is a YARP device opening and attaching itself to the remote control boards of all parts of the iCubGazeboV2_5 robot. It also opens and attaches itself to the wholeBodyDynamics device. A transformServer is also opened in order to publish transforms to ROS \tf topic.

However, I tested the WalkingModule also with only the essential components, which is only wholeBodyDynamics, and the falling behavior seems to be reproducible.

GiulioRomualdi commented 5 years ago

Hi @prashanthr05, the error

[ERROR][evaluateZMP] The total z-component of contact wrenches is too low. 
[ERROR][updateModule] Unable to evaluate the ZMP.

happens when the z component of both feet wrenches is lower than 0 newtons. Just to be sure, have you reset the F/T offset before running the controller?

yarp rpc /wholeBodyDynamics/rpc
>> resetOffset all
prashanthr05 commented 5 years ago

Just to be sure, have you reset the F/T offset before running the controller?

Yes. I reset the FT sensor offsets usually before running the controller. However I use the following,

 yarp rpc /wholeBodyDynamics/rpc
 >> calibStanding all

I do not know the essential difference between calibStanding all and resetOffset all. I also do not remember why I chose to use the former. From the source code, they seem to zero() different sets of buffers.

I suppose it is recommended to use resetOffset all. I will stick with that procedure.

Anyhow, even with resetOffset all, the robot still falls.

S-Dafarra commented 5 years ago

I am afraid it may just be a problem of bad tuning, made worse by synchronization problems. Do you have any animation that shows the fall?

prashanthr05 commented 5 years ago

I do not know if this view is straightforward to observe the imbalance, but this is one of the trials where the robot falls

fbe1

S-Dafarra commented 5 years ago

The real time factor is 0.1 or 0.3? Maybe if it is so small it may be worthwhile to connect to the gazebo clock (it has to be implemented in the controller code). Or to try directly on the robot :grin:

Another simple test you could try is to use a set of parameters more similar to those used for iCubGenova04.

S-Dafarra commented 5 years ago

Another simple test you could try is to use a set of parameters more similar to those used for iCubGenova04.

I just tried, but it is not making it any better. Maybe @GiulioRomualdi can give some more advice on the tuning.