csiro-robotics/syropod_highlevel_controller

OpenSHC: A Versatile Multilegged Robot Controller

Syropod High-level Controller

Syropod High-level Controller (SHC) is a versatile controller capable of generating body poses and gaits for quasi-static multilegged robots. This ROS package implemented in C++ can be easily deployed on legged robots with different sensor, leg and joint configurations. SHC is designed to generate foot tip trajectories for a given gait sequence, step clearance, step frequency and input body velocity. Input sensors such as IMU and joint effort feedback can be utilised by the controller to provide robust trajectories even in inclined and uneven terrain. SHC was developed at CSIRO’s Robotics and Autonomous Systems Group to support its ongoing legged robot locomotion research. It is now being released as an open-sourced package, along with tutorials, for the benefit of the wider community as OpenSHC. The banner image and the video clip below shows a number of CSIRO's legged robots running SHC.

Table of Contents

• Getting Started
• Features
• Config Files
• Nodes
• Changelog
• Authors
• License
• Contributing
• Issues

Getting Started

If you haven't looked at the tutorials for using OpenSHC, see OpenSHC Tutorials.

Please refer to the readme in the launch folder of individual platforms for information on setting up the platform for use with OpenSHC.

Video overview of OpenSHC:

Requirements

• Ubuntu 18.04 LTS and ROS Melodic; or
• Ubuntu 20.04 LTS and ROS Noetic

Dependencies

• OpenSHC requires a robot specific configuration and launch repository such as Bullet Syropod to run. Please refer to that readme for more information.

Installation

mkdir -p openshc_ws/src
cd openshc_ws/src
git clone https://github.com/csiro-robotics/syropod_highlevel_controller.git
cd ..
catkin build

Publications

The details of OpenSHC is published in the following article:

B. Tam, F. Talbot, R. Steindl, A. Elfes and N. Kottege, "OpenSHC: A Versatile Multilegged Robot Controller," in IEEE Access, vol. 8, pp. 188908-188926, 2020, doi: 10.1109/ACCESS.2020.3031019.

How to cite

@article{tam2020openshc,
    author={Benjamin Tam and Fletcher Talbot and Ryan Steindl and Alberto Elfes and Navinda Kottege},
    title={OpenSHC: A Versatile Multilegged Robot Controller},
    journal={IEEE Access},
    year={2020},
    volume={8},
    number={},
    pages={188908-188926},
    doi={10.1109/ACCESS.2020.3031019}
}

Features

• Fully configurable for a variety of platform designs with differing physical characteristics, including up to eight legs each with up to 5 degrees of freedom.
• Four dynamically switchable gait options (Wave, Amble, Ripple and Tripod) plus ability to easily design custom gaits. (see config/readme.md)
• User defined body clearance, step clearance and step frequency.
• Manual body posing in 6 degrees of freedom.
• Manual leg manipulation - select up to two legs simultaneously and toggle manual manipulation of either tip position in Cartesian space or direct control of joint positions (3DOF legs only).
• Choice between two modes of start-up:
  • Direct mode which move leg tip positions linearly from initial position to default walking stance positions.
  • Full chain of start-up/shutdown sequences which allow a Syropod to start from a 'packed' state and generate a sequence to stand up off the ground into its default walking stance. Similarly able to shut-down and transition back to a packed state.
• Optional impedance controller with dynamic leg stiffness to ensure leg contact with ground and offer a degree of rough terrain walking ability.
• Optional IMU body compensation which uses IMU data to keep body horizontally level at all times.
• Optional inclination compensation which strives to keep body centre of gravity over the estimated centre of support whilst walking on inclined planes.
• Optional bespoke automatic body posing system which poses each robot leg cyclically as defined by auto-pose parameters. (see config/readme.md)
• Cruise control mode which may either force a constant predefined input velocity for the Syropod or set the current input velocity as constant.
• Auto navigation mode which when combined with the correct sensing capabilities and running syropod_auto_navigation node, give autonomous navigation with obstacles avoidance to an input waypoint.

Config Files

For information on parameters see readme.md in config folder.

Nodes

syropod_highlevel_control

Subscribed Topics

syropod_remote

• System State:
  • Description: The desired state of the entire Syropod High-level Controller system.
  • Topic: /syropod_remote/system_state
  • Type: std_msgs::Int8
• Robot State:
  • Description: The desired state of the robot.
  • Topic: /syropod_remote/robot_state
  • Type: std_msgs::Int8
• Desired Velocity:
  • Description: The desired body velocity of the robot.
  • Topic: /syropod_remote/desired_velocity
  • Type: geometry_msgs::Twist
• Desired Pose:
  • Description: The desired body pose of the robot.
  • Topic: /syropod_remote/desired_pose
  • Type: geometry_msgs::Twist
• Posing Mode:
  • Description: The desired manual body posing input mode.
  • Topic: /syropod_remote/posing_mode
  • Type: std_msgs::Int8
• Pose Reset Mode:
  • Description: The desired manual body pose reset mode.
  • Topic: /syropod_remote/pose_reset_mode
  • Type: std_msgs::Int8
• Gait Selection:
  • Description: The desired gait selection for the walk controller of the robot.
  • Topic: /syropod_remote/gait_selection
  • Type: std_msgs::Int8
• Cruise Control Mode:
  • Description: The desired cruise control mode.
  • Topic: /syropod_remote/cruise_control_mode
  • Type: std_msgs::Int8
• Auto-Nagivation Mode:
  • Description: The desired auto-navigation mode.
  • Topic: /syropod_remote/auto_navigation_mode
  • Type: std_msgs::Int8
• Primary Leg Selection:
  • Description: The desired leg selected for primary manipulation.
  • Topic: /syropod_remote/primary_leg_selection
  • Type: std_msgs::Int8
• Primary Leg State:
  • Description: The desired state of the leg selected for primary manipulation.
  • Topic: /syropod_remote/primary_leg_state
  • Type: std_msgs::Int8
• Primary Tip Velocity:
  • Description: The desired tip velocity for the leg selected for primary manipulation.
  • Topic: /syropod_remote/primary_tip_velocity
  • Type: geometry_msgs::Point
• Secondary Leg Selection:
  • Description: The desired leg selected for secondary manipulation.
  • Topic: /syropod_remote/secondary_leg_selection
  • Type: std_msgs::Int8
• Secondary Leg State:
  • Description: The desired state of the leg selected for secondary manipulation.
  • Topic: /syropod_remote/secondary_leg_state
  • Type: std_msgs::Int8
• Secondary Tip Velocity:
  • Description: The desired tip velocity for the leg selected for secondary manipulation.
  • Topic: /syropod_remote/secondary_tip_velocity
  • Type: geometry_msgs::Point
• Parameter Selection:
  • Description: The desired parameter selection for possible adjustment.
  • Topic: /syropod_remote/parameter_selection
  • Type: std_msgs::Int8
• Parameter Adjustment:
  • Description: The desired adjustment of the selected parameter (increment/decrement).
  • Topic: /syropod_remote/parameter_adjustment
  • Type: std_msgs::Int8

syropod_manipulation

• Primary Leg Tip Pose:

  • Description: The desired pose for the leg selected for primary manipulation within Cartesian space.
  • Topic: /syropod_manipulation/primary_tip_pose
  • Type: geometry_msgs::Pose

• Secondary Leg Tip Pose:

  • Description: The desired pose for the leg selected for secondary manipulation within Cartesian space.
  • Topic: /syropod_manipulation/secondary_tip_pose
  • Type: geometry_msgs::Pose

Motor and Sensor Inputs

• IMU Data:
  • Description: The input data from onboard IMU.
  • Topic: /imu/data
  • Type: sensor_msgs::Imu
• Tip State Data:
  • Description: Custom message containing force/torque and range sensor data at the tip of each leg.
  • Topic: /tip_states
  • Type: syropod_high-level_controller::TipState
• Joint State Data:
  • Description: The actual state of joints within the Syropod as published by hardware.
  • Topic: "/joint_states
  • Type: sensor_msgs::JointState

Published Topics

Dynamixel Motor Interface

• Desired Joint Position (per joint)
  • Description: Desired joint position for each individual joint.
  • Topic: /syropod/*LEG_ID*_*JOINT_ID*_joint/command
  • Type: std_msgs::Float64
• Desired Joint State (combined)
  • Description: Desired joint state array for all joints.
  • Topic: /desired_joint_state
  • Type: sensor_msgs::JointState

Miscellaneous

• Body Velocity:
  • Description: The desired velocity of the robot body.
  • Topic: /syropod_highlevel_controller/body_velocity
  • Type: geometry_msgs::Twist
• Body Pose:
  • Description: The desired pose of the robot body.
  • Topic: /syropod_highlevel_controller/pose
  • Type: geometry_msgs::Twist
• ASC Hexapod State: (To be removed)
  • Description: ASC Hexapod specific message for toggling of magnetic feet (bool of whether each leg is in stance state or not)
  • Topic: /leg_state_*LEG_ID*_bool
  • Type: std_msgs::Bool
• Leg State:
  • Description: The leg state message combines several leg specific data for use in debugging.
    • header: Header with timestamp
    • name: Leg designation
    • walker_tip_pose: Desired tip pose generated from the walk controller (walk_plane frame)
    • target_tip_pose: Future desired tip pose at end of swing period (walk_plane frame)
    • poser_tip_pose: Desired tip pose generated from the pose controller (base_link frame)
    • model_tip_pose: Desired tip pose finalised by the model inverse/forward kinematics (base_link frame)
    • model_tip_velocity: Desired tip velocity finalised by the model inverse/forward kinematics (model).
    • joint_positions: Array of desired joint positions for each joint.
    • joint_velocities: Array of desired joint velocities for each joint.
    • joint_efforts: Array of desired joint efforts for each joint.
    • stance_progress: Progress along stance state (0.0->1.0 OR -1 if not in stance)
    • swing_progress: Progress along swing state (0.0->1.0 OR -1 if not in swing)
    • time_to_swing_end: Time until this leg completes swing period.
    • pose_delta: The estimated change in pose of the walk_plane frame with regard to world frame from current time to end of swing period.
    • auto_pose: The automatic cyclic posing applied to this leg from the auto-pose system.
    • tip_force: Tip force vector used in impedance control calculations
    • admittance_delta: Vertical tip position offset - output of admittance control.
    • virtual_stiffness: Current virtual stiffness used in admittance control calculations
  • Topic: /shc/*LEG_ID*_leg/state
  • Type: syropod_highlevel_controller::LegState (custom message)

Changelog

See CHANGELOG.md for release details.

Authors

• Fletcher Talbot
• Ryan Steindl
• Thomas Molnar
• Thomas Lowe
• Oshada Jayasinghe
• Navinda Kottege

License

This project is licensed under the CSIRO Open Source Software Licence Agreement (variation of the BSD / MIT License) - see the LICENSE file for details.

Contributing

Thank you for being interested in contributing. We encourage the community to create new features and to fix bugs. If it is a new feature, please create a new issue via the Issue Tracker and describe the intended feature so that we can discuss the implementation. Else, search for the existing issue and comment on your proposed work. Please fork the repo, work on your changes and then send a pull request.

Issues

Please report bugs using Issue Tracker or contact us via email [email protected].

syropod_highlevel_controller's Issues

Problems over custom robot configuration

Hi,
I'm trying to use your controller over a full custom Hexapod, but i really don't know where to start.
I read all the read-me but i can't still understand how to set-up everything, i would like some suggestion and step on how to complete the set-up, moreover i couldn't understand how to integrate the controller since it seems that the doc for configuring it is missing.

Thanks for the support.

Error while launching the gazebo simulation for the bullet

Hi all, Thank for this amazing job, here is my issue:

I get this error when launching the gazebo simulation as presented in the shc_tutorial:
https://github.com/csiro-robotics/shc_tutorials/blob/master/shc_pc_gazebo_simulation.md
(this error for each joint)

Loading controller: CR_tarsus_joint
[ERROR] [1603356651.666804983]: Could not load controller 'CR_tarsus_joint' because controller type 'effort_controllers/JointPositionController' does not exist.
[ERROR] [1603356651.666897035]: Use 'rosservice call controller_manager/list_controller_types' to get the available types
[ERROR] [1603356652.668200]: Failed to load CR_tarsus_joint

How can I fix this ? In the gazebo launch file we have this:

and I am not sure the pkg controller_manager is existing somewhere. I think this may be the issue in that case.
I installed all the dynamixel pkg included in the csiro-robotics git.

Thank you for your answers
Cheers

PID Tuning for new robot model

Hi again,
quick question, do you have method to tune the PIDs of the motors in the gazebo simulation for bullet? I implemented a new Xacro for an other robot model and I am having hard times tuning the PID, the robot is too slow or too bouncy and can't keep up with the Rviz model (line skeleton).
Thank you by advance :)
Cheers,
Pierre

Touchdown detection and Rough terrain mode

Hi all,
I have a question concerning rough terrain mode, I am trying to do touchdown detection using FSR and I am not sure about how I am supposed to link the information to the openSHC controller. I guess information should be published to the /tip_states topic.. Can it be only published in the "force" section of that message type ? or do I need to put extra information ?
So far I can get the walking plan to move up using a random force value on z. Is it important to play around with the step depth and the stiffness and damping parameters to do rough terrain locomotion ?
Thanks a lot !

questions on ground touchdown

Hi guys, I'm really enjoying the fruits of your hard work. I've successfully run the openshc stack on my custom hexapod (video here: https://www.youtube.com/watch?v=gDRpkwpoU_c). It has 18 Dynamixel RX28 servos, and runs on a Jetson Nano.

I've been trying to wrap my head around the admittance controller feature of the openshc stack. Here is my understanding of what it does: it can either calculate the force at the tip from the servos' load feedback, or measure them directly through additional sensors. While walking, when a leg touches (i.e. tip force > threshold) the ground earlier or later than expected (as it would on a flat surface), it will hold the z position of that leg, so that 1) not one leg bears too much load and 2) to try to keep the body relatively level.

At first I tried playing around with the parameters for the admittance controller with use_joint_effort set to true, but it didn't seem to have any effect on the walking behaviour. I had a brief look at the code for how it's calculated, and it seems it's only initialized to zero vectors here

syropod_highlevel_controller/src/model.cpp

Line 182 in 26c0c71

tip_force_calculated_ = Eigen::Vector3d::Zero();

and it doesn't actually seem to be computed from the servo load values. Please point me in the right direction if I missed it.

The next thing I tried was to implement a simple "force" sensor using tactile push button switches attached to the feet. I attached them to an arduino that communicates with the host system through serial, and wrote a little ROS node that publishes the tip sensor data to the topic /tip_states. I tried publishing them at 100Hz and 1000Hz. Since it's just a push button switch, the published message looks like this with only the z component of the force being nonzero:

header:       
  seq: 542
  stamp: 
    secs: 1598803970
    nsecs: 978107929
  frame_id: ''
name: 
  - AR_
  - BR_
  - CR_
  - CL_
  - BL_
  - AL_
wrench: 
  - 
    force: 
      x: 0.0
      y: 0.0
      z: 0.0
    torque: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    force: 
      x: 0.0
      y: 0.0
      z: 1.0
    torque: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    force: 
      x: 0.0
      y: 0.0
      z: 0.0
    torque: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    force: 
      x: 0.0
      y: 0.0
      z: 1.0
    torque: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    force: 
      x: 0.0
      y: 0.0
      z: 0.0
    torque: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    force: 
      x: 0.0
      y: 0.0
      z: 1.0
    torque: 
      x: 0.0
      y: 0.0
      z: 0.0
step_plane: []

• The values in the field name are chosen so that

  syropod_highlevel_controller/src/state_controller.cpp

  Line 1625 in 26c0c71

  std::string leg_name = tip_name.substr(0, tip_name.find("_"));

  will correctly recognize them.
• I wasn't sure how to construct the step_plane values, so I left them empty.
• the wrench.force.z is set to 1 if the tip switch is pressed and 0 when not.

In my understanding, this should be sufficient to get that part of the code rolling, but it still doesn't seem to be doing what I think it's supposed to be doing. For example, it does these little "jumps" when stepping onto obstacles about 5cm tall, like you can see around 0:12 on the video (https://youtu.be/gDRpkwpoU_c?t=12).

Any pointers would be appreciated.

Cheers!

Some question

good day
I was very interested in your project, and I want to assemble it at home. I am interested in the connection diagram, the list of used equipment and components.
Could I use budget servos?

Auto navigation node

Hey guys, is the code for the auto navigation mode mentioned in the documentation available?