bdaiinstitute / spot_ros2

ROS 2 driver package for Boston Dynamics' Spot
Other
180 stars 62 forks source link

cmd_vel movement is unstable #394

Closed T-Lemmel closed 5 months ago

T-Lemmel commented 5 months ago

Hello, I am trying to have the spot moving with cmd_vel sent by a controller but it's being difficult due to various factors :

This is causing a lot of problems since the controller starts with speeds < 0.3m/s & 0.3 rad/s for the transition phase. It tried scaling everything to a minimum of 0.3 & -0.3 but this makes the robot extremely unstable.

I there a way to make these movement smoother, is the 0.3m/s & 0.3rad/s limit a safety feature coded or mecanical limitation of the motors ?

You can use this command in a terminal with various values to see what i mean.

ros2 topic pub Argos/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" -r10

Thanks a lot in advance for the Help, Tom

(edit : i increased the cmd_duration value in my spot_logs.yaml, that makes the movement way smoother but i still have problems with command with "small" absolute values)

khughes-bdai commented 5 months ago

Glad that changing cmd_duration smooths things out! I get fairly smooth behavior with a value of around 0.25. Here's my config yaml for reference.

/**:
  ros__parameters:
    robot_state_rate: 30.0
    metrics_rate: 0.04
    lease_rate: 1.0
    image_rate: 15.0
    auto_claim: False
    auto_power_on: False
    auto_stand: True
    deadzone: 0.05
    estop_timeout: 9.0
    start_estop: False
    cmd_duration: 0.25

I was able to publish cmd_vel with linear components as small as +/- 0.13 and angular +/- 0.13 using ros2 topic pub at 10 Hz. I couldn't find a ton of information about this from BD, but I think there is some lower bound with the velocities you can send via the robot command builder which is probably about this level (https://dev.bostondynamics.com/python/bosdyn-client/src/bosdyn/client/robot_command.html#bosdyn.client.robot_command.RobotCommandBuilder.synchro_velocity_command). Another thing to keep in mind is the relationship bewteen cmd_duration and the publish rate of your cmd_vel controller. If you are publishing slower than the duration of the cmd_vel command, this will result in jerky motion. If your cmd_duration is 0.25, you would need to be publishing to cmd_vel at 4Hz minimum for smooth motion (but in practice, probably around 8-10 Hz for a reasonable buffer).

T-Lemmel commented 5 months ago

Thanks for the help !

Now that the movement is fairly smooth, i'll see if i can make it works despite these lowers bounds but i doubt it since my controller is more thought for robots that do not have these specificities and therefore it keeps publishing really low values at the beginning of the movement. Anyway it's good to know that those bounds are really a limitation before trying to work around it. Thanks for the insight on cmd_vel frequency/cmd_duration :+1:

I'll close the issue now, thanks