Driving robot with controllers

[AlexGisi]

Hello, thank you for the excellent library. I am trying to drive a Clearpath Jackal with the controllers provided in that repo, but the robot is not moving. I was wondering if I am doing something obviously wrong.

Here are the steps I took:

1. Generate a URDF from the xacro provided here.
2. Edit mujoco_simsrc/config/robot.yaml, making the changes

add_odom_joints: true
root_frame_id: odom

4. Start the controller with roslaunch jackal_control control.launch.
5. Start the simulation with roslaunch mujoco_sim mujoco_sim.launch robot:=/my/path/test.urdf use_urdf:=true.
6. Observe the odometry is working as expected (rostopic echo /odometry/filtered) and the /mujoco_sim node is subscribed to the /jackal_velocity_controller/cmd_vel topic:

> rostopic info /jackal_velocity_controller/cmd_vel
Type: geometry_msgs/Twist

Publishers: 
 * /twist_mux (http://alex-laptop:39041/)

Subscribers: 
 * /mujoco_sim (http://alex-laptop:39363/)

7. But when publishing velocity commands:

   rostopic pub /jackal_velocity_controller/cmd_vel geometry_msgs/Twist "{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" -r 10

   the robot does not respond. The robot can still be moved directly:

rostopic pub /jackal/cmd_vel geometry_msgs/Twist "{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}" -r 10

Would you have any insight into where I'm misguided?

[HoangGiang93]

Did you check with rostopic echo /jackal/cmd_vel when you do rostopic pub /jackal_velocity_controller/cmd_vel geometry_msgs/Twist "{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" -r 10 ? Could you also do rosnode info /mujoco_sim to see if it actually connects to your topic?

[AlexGisi]

Thanks for the response. I now see that the original twist_mux publisher /jackal_velocity_controller/cmd_vel must be remapped to /jackal/cmd_vel in order to be received by the mujoco_sim node.

My underlying question related to simulating the response of the jackal hardware to the velocity commands. Presently, when I published to /jackal/cmd_vel, the robot instantly obtains the requested speed (e.g. 10 m/s). Clearly, the wheel contacts are not being simulated (they would slip) nor are the wheel joints (the requested speeds exceed the joint limits).

Examining the code, I do not understand how the controllers (e.g. diff_drive_controller) interface with the hardware interface (as shown in the picture below). Is that interaction hidden?

Maybe I have some misconceptions on how to use the system!

[HoangGiang93]

Ok first off, the /cmd_vel has nothing to do with the ros_control. It's something I implemented to simplify the base controller without having another "controller" for the wheel to move the base according to the commanded velocity. The principle is also simple, for the diff_drive_controller, I only added three virtual joints (odom_lin_x, odom_lin_y and odom ang_z) to the base, making it moving with three degrees of freedom. The rest is mapping the control signal to the velocity.

About how the controllers interface with the hardware interface. If you use e.g. joint_trajectory_controller, you can see that by changing the type, you can make it position, velocity or effort-based controller. The actual implementation is indeed hidden in the hardware interface (hence it calls "hardware interface"), making it flexible to change in runtime.

For my package, here is how I implemented the hardware interface.

Hope it helps.

[AlexGisi]

I see. I identified one source of my problem, which was the model file being generated by the urdf->xml compilation process. That model was not suitable for my purpose (e.g. it lacked actuators at the wheels). I replaced it with a hand-make jackal model file, which has four hinge joints representing wheels.

This means when the virtual joints are added, the robot has more than 6-dof which is unacceptable to mujoco. Disabling the virtual odometry joints allows the robot to spawn, but then I am not sure how to communicate with the controllers.

I used the following robot and world files:

jackal_robot.xml jackal_world.xml

[HoangGiang93]

you can control the wheel using the velocity_controller interface from ros_control. You just need to specify the yaml file including all wheels and the corresponding controllers.

[AlexGisi]

I can apparently load the diff_drive_controller (which is the controller I want to use), but I do not understand the resulting behavior. My control.yaml is

jackal_joint_publisher:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50

jackal_velocity_controller:
  type: "diff_drive_controller/DiffDriveController"
  left_wheel: ['front_left_wheel', 'rear_left_wheel']
  right_wheel: ['front_right_wheel', 'rear_right_wheel']
  publish_rate: 50
  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
  twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
  cmd_vel_timeout: 0.25

  k_l: 0.1
  k_r: 0.1

  # Odometry fused with IMU is published by robot_localization, so
  # no need to publish a TF based on encoders alone.
  enable_odom_tf: false

  # Wheel separation and radius multipliers
  wheel_separation_multiplier: 1.5 # default: 1.0
  wheel_radius_multiplier    : 1.0 # default: 1.0

  # Velocity and acceleration limits
  # Whenever a min_* is unspecified, default to -max_*
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 2.0   # m/s
      has_acceleration_limits: true
      max_acceleration       : 20.0   # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 4.0   # rad/s
      has_acceleration_limits: true
      max_acceleration       : 25.0   # rad/s^2

But the commands I send don't seem to be subject to any control loop or hardware simulation. They are just directly enacted upon the robot state.

This occurs whether or not the controllers are loaded: I attached videos below of publishing an "unrealistic" command, which is immediately met.

Without controllers: no-controller.webm

With controllers: with-controller.webm

Is this really the expected behavior?

[HoangGiang93]

Looks like so, did you disable the virtual odometry joints? You can set angular velocity lower and see if the wheels are rotating. The unit is radian, so 55rad/s is quite big.

[AlexGisi]

The above example included the virtual odometry joints. I was just surprised the control limits set in control.yaml aren't being respected.

[HoangGiang93]

Well if you want to control the base via the wheel, then you should exclude the virtual odom joints, otherwise your wheels dont do anything

[HoangGiang93]

Set the add_odom_joints to false in the yaml file like in this.

[AlexGisi]

Hi Giang,

Yes, it seems the add_odom_joints must be false in this case, making the config file comment a little misleading. Perhaps the comment should be clarified?

The ultimate issue was with line 651 in mj_ros.cpp, which is the predicate for adding controlled joints:

if (controller_state.state.compare("running") == 0 && (controller_state.type.find("position_controllers") != std::string::npos || controller_state.type.find("velocity_controllers") != std::string::npos || controller_state.type.find("effort_controllers") != std::string::npos)

The Jackal's diff_drive_controller/DiffDriveController was not triggering this statement, so I had to add controller_state.type.find("diff_drive_controller/DiffDriveController") != std::string::npos to the list of conditions.

Also, the order of running the launch files was important, and perhaps should be documented somewhere:

roslaunch mujoco_sim mujoco_sim.launch robot:=<path to xml>
roslaunch jackal_description description.launch
roslaunch jackal_control control.launch

Finally, I'd like to note the sensornoise="disable" tag in line 3 of empty.xml causes an error on loading the model and is not mentioned in the xml documentation, so should probably be removed.

I could make a pull request for these changes if you would like?

[HoangGiang93]

I'm glad it worked out for you. Please make a pull request.