HPP ROS2 CONTROL

Author : Loan Bernat. l.bernat@sileane.com

Compatibility :

hpp_interface	hpp_control	Robot tested
1.0.2	1.0.2	fr3
1.0.1	1.0.1	fr3
1.0.0	1.0.0	fr3, fer*

(*) fer must work, you just need to change the urdf to include the mounted camera, if using the ADREM ones.

Description

This repository contains two ROS2 packages :

• hpp_control : The ros2 control package to interface the software Humanoid Path Planner (HPP) easly with the ros2_controller Joint Trajectory Controller.
• hpp_interface : The ros2 package with message, service and action type for hpp_control.

The repository also provides a Dockerfile which allow you to have a base image already tested to run those packages on a separated container from the rest of your code. See Docker Integration for more details.

⚠️ WARNING : These packages are thinked to be used with panda arm fer (Franka Emika Research) or fr3 (Franka Research 3) in the ADREAM environment. To use on differents robots or environment, you need to change loaded URDF and SRDF and modify the extractRobotConfig function in utils.py.

❗ INFORMATION : To control robot in real time, you need to have other ros2 node launched and a Joint Trajectory Controller activated.

Usage

In the hpp_control package you have two nodes and two exemple scripts.

HPP Scripts

• move_one_box.py : Exemple of script to create a HPP problem and solve it.

• move_cartesian.py : Example of script to create an handle in the space and generate a configuration of the robot with the end-effector positioned at the handle.

From these script (using interactive python python3 -i **.py) you can also acess functions from utils.py. Those functions allow you to rapidly test and debug your script without the need to use a separated node. (Your simulation or real robot launch need to running and using Joint Trajectory Controller from ros2_control)

• getRobotState() connect to the robot and return its configuration.
• sendingTraj(problesolver, pathID, derivative, ARM_ID) send the path corresponding to the pathID to a ARM_ID robot with only position (derivative=0), position + speed (derivative=1), position + speed + acceleration (derivative=2)

❗ INFORMATION : For more documentation on HPP's function and script, please refer to the official website.

ROS2 Nodes

Both nodes take a ros parameter : arm_id ; Which correspond to the type of panda you are using (fer or fr3).

solving_hpp : Standalone node to plan a trajecotry with HPP. Can take a configuration or an end-effector pose in input.

You can interact using ros2 actions. Thes actions will automatically send the trajectory to the Joint Trajectory Controller at this topic /joint_trajectory_controller/follow_joint_trajectory and will wait for the end of the exucution before ending :

ros2 action	hpp_interface type	Description
/hpp_node/plan_trajectory_to_q_config	ConfigSolve	Plan Trajectory from q_init to q_goal
/hpp_node/plan_trajectory_to_ee_pose	PoseSolve	Plan trajectory from q_init to a 7DOF pose
/hpp_node/inv_trajectory	InvTrajSolve	Plan trajectory to return to the previous Qinit

If you don't want to wait the end of the trajectory, you can simply publish the desired pose on the topic /hpp_node/fast_plan_to_q to automatically calcul and send the trajectory from the current position of the robot to the given configuration.

sending_hpp : Sending an already calculated trajectory to the ros2 controllers.

You can interact with the node using the /hpp_node/sendTrajectory service based on the HppSendTrajectory service type.

To use this node, you need to have a corbaserver running with trajectory already solved. For example, you can use the node to send the trajectory computed from move_one_box.py

Docker Integration

To use those packages in a container, you just need to mount folders hpp_control and hpp_interface direclty into the container, setting network:'host' (to simplify) and setting the same ROS_DOMAINE_ID that the rest of your project.

This is an exemple of use inside a docker-compose file :

hpp:
    build: 
      context: .
      dockerfile: hpp_jointtrajcontrol/Dockerfile
    network_mode: "host"
    container_name: hpp
    command: /bin/bash
    tty: true
    stdin_open: true
    volumes:
      - ./hpp_jointtrajcontrol/hpp_control:/ros2_ws/src/hpp_control
      - ./hpp_jointtrajcontrol/hpp_interface:/ros2_ws/src/hpp_interface
      - /tmp/.X11-unix:/tmp/.X11-unix
      - /dev:/dev
    environment:
      QT_X11_NO_MITSHM: 1
      DISPLAY: $DISPLAY
      ROS_DOMAIN_ID: 10
      RMW_IMPLEMENTATION: rmw_cyclonedds_cpp
    cap_add:
      - SYS_NICE
    ulimits:
      rtprio: 99
      rttime: -1
      memlock: 8428281856

If you are not familiar with docker or docker compose. Check the official documentation

Support

Please contact me at l.bernat@sileane.com

Contributing

You can contribute to this project by opening a merge request.

License

Appache-2.0