ipa320 / cob_robots

www.care-o-bot.org
Apache License 2.0
69 stars 132 forks source link

base_controller not running correctly in simulation #641

Closed fmessmer closed 7 years ago

fmessmer commented 7 years ago

as reported by @ipa-bfb they seem to be drifting...needs investigation....

floweisshardt commented 7 years ago

are you sure this is the base_controller? What I observed was just the simulated robot model slightly drifting in gazebo (independent of wheel positions and controller)

bbrito commented 7 years ago

try to use cob_teleop and see if it works...

floweisshardt commented 7 years ago

sorry, ignore my comment. Seems as the recently introduced commits broke driving the robot in gazebo. we'll have to investigate.

fmessmer commented 7 years ago

Why are the steer_target_velocity that high? Is this rad/sec? @ipa-mdl

rostopic echo /base/twist_controller/wheel_commands 
header: 
  seq: 52189
  stamp: 
    secs: 52
    nsecs: 923000000
  frame_id: ''
drive_target_velocity: [0.0, 0.0, 0.0]
steer_target_velocity: [7.60710124957176, -10.0, -10.0]
steer_target_position: [0.0, -3.141592653589793, 0.0]
steer_target_error: [1.395496564844115, -2.722543136645081, -3.048259837630063]

Is the non-zero value in the steer_target_position some kind of "homing position"? Where is this defined? It's this line for cob4 and this line for raw3

In gazebo all joints are spawned at zero position...thus there is a big initial steer_target_error...thus the steer_target_velocity are high.... We either need to:

mathias-luedtke commented 7 years ago

they seem to be drifting...needs investigation....

I had the same problem when we implemented the dynamic-reconfigure feature. For some reason it just works with you setup.

Is this rad/sec?

Of course ;)

or we try to spawn the wheels in gazebo at the correct starting position

+1 The starting postions point away from the base link. That's why the back wheel has a neutral position of 180 degrees

fmessmer commented 7 years ago

There seems to be something else: I can set the steer_joints to the desired neutral position (cob4-2) using

rosservice call /gazebo/set_model_configuration '{ model_name: "robot", urdf_param_name: "robot_description", joint_names: [ "fl_caster_rotation_joint", "b_caster_rotation_joint", "fr_caster_rotation_joint" ], joint_positions: [ 0.0, -3.141592653589793, 0.0 ] }'

the joint_states are then at the desired position, but still the wheels drift again...I also cannot use roslaunch cob_teleop teleop_keyboard.launch

Even if I comment the steer_neutral_position from the yaml, the drifting occurs and I cannot drive...

For some reason it just works with you setup.

@ipa-mdl, I don't know why it seemed to work on my setup....

floweisshardt commented 7 years ago

this commit seems to have broken moving the base in simulation: https://github.com/ipa320/cob_common/commit/6f6ccc2552e42d57c851997e4bf825fa5ea1c3d0 If I revert this, simulation is OK again. @ipa-mdl: any idea what might cause the troubles?

floweisshardt commented 7 years ago

setting the effort limit back to 100 fixes the issue. see https://github.com/ipa320/cob_common/pull/215

floweisshardt commented 7 years ago

@ipa-bfb: FYI fixed.

bbrito commented 7 years ago

Awesome! Thanks ;)