Connecting Giskard to a Robot

Connecting Giskard to a Robot#

This tutorial connects Giskard to a robot that is driven by ros2_control, using a Universal Robots UR5e as the example. No real arm is needed: the Universal Robots driver can run on mock hardware, which integrates the commanded velocities and reports the result as joint states. The setup is the same one the driver uses for a real arm.

Prerequisites#

A ROS workspace as described in the monorepo README, sourced in every terminal used below.
The Universal Robots driver: sudo apt install ros-jazzy-ur-robot-driver.

What Giskard needs from a robot#

Giskard is configured with four objects:

Object	What it decides
`WorldConfig`	Which robot description is loaded and how the robot is attached to the world.
`RobotInterfaceConfig`	Whether a motion is only simulated (`StandAloneRobotInterfaceConfig`) or sent to the robot (a `ClosedLoopRobotInterfaceConfig`), and for a real robot where its state is read from and where commands are sent.
`GiskardServerConfig`	The goal lifecycle and the debug output of the motion server.
`QPControllerConfig`	The control frequency and the tuning of the controller.

To command a robot, Giskard needs two things from it:

a joint_state_broadcaster/JointStateBroadcaster, which publishes the joint states;
a velocity_controllers/JointGroupVelocityController, which accepts joint velocities.

Both have to be active. Giskard asks the controller manager which controllers are running and connects to these two kinds on its own, so the joints and topics do not have to be listed by hand.

The configuration#

#!/usr/bin/env python
from dataclasses import dataclass

from giskardpy.middleware.ros2.giskard import Giskard
from giskardpy.middleware.ros2.robot_interface_config import (
    ClosedLoopRobotInterfaceConfig,
)
from giskardpy.middleware.ros2.ros2_interface import get_robot_description
from giskardpy.middleware.ros2.server_config import GiskardServerConfig
from giskardpy.model.world_config import WorldWithFixedRobot
from giskardpy.qp.qp_controller_config import QPControllerConfig


@dataclass
class ControllerManagerInterface(ClosedLoopRobotInterfaceConfig):
    """
    Reads and commands a robot through the active controllers of its ros2_control
    controller manager.
    """

    def setup(self) -> None:
        self.discover_interfaces_from_controller_manager()


def create_giskard() -> Giskard:
    """
    Build a Giskard that commands the robot described on ``/robot_description`` through
    its controller manager.

    .. note:: The ros node has to be initialized first, because the robot description is
        read from its topic.
    """
    return Giskard(
        world_config=WorldWithFixedRobot(urdf=get_robot_description()),
        robot_interface_config=ControllerManagerInterface(),
        server_config=GiskardServerConfig(),
        qp_controller_config=QPControllerConfig(target_frequency=50),
    )


def main() -> None:
    """
    Run the Giskard :func:`create_giskard` builds as a ros node until ROS shuts down.
    """
    Giskard.launch(create_giskard)


if __name__ == "__main__":
    main()

WorldWithFixedRobot loads the robot description and fixes the robot to the world. It suits an arm that is bolted down. The description is read from the /robot_description topic, so Giskard uses the same model as the driver. The section “Where the robot description comes from” below says when to read it from a file instead.
ControllerManagerInterface asks the controller manager for its active controllers. It reads the joint states from the broadcaster and sends velocities to the velocity controller. Because it is a ClosedLoopRobotInterfaceConfig, Giskard sends its commands to the robot in real time and controls only the joints a controller commands. Passing StandAloneRobotInterfaceConfig() instead simulates the motion without hardware.
target_frequency is the control frequency in hertz.

Because nothing in this file names a joint or a topic, it works unchanged for any fixed-base robot whose controller manager runs these two controllers.

Robots with a robot class or several controller managers#

A robot that has a robot class in semantic_digital_twin.robots already declares the topics its parts publish their state on, and it may run more than one controller manager. Tracy runs one per arm, each in its own namespace:

@dataclass
class TracyVelocityInterface(ClosedLoopRobotInterfaceConfig):
    """
    Commands both arms of Tracy through their forward velocity controllers.
    """

    def setup(self):
        self.sync_robot_parts()
        for controller_manager in TracyControllerManager:
            self.discover_interfaces_from_controller_manager(
                controller_manager, whitelist=[TracyController.FORWARD_VELOCITY]
            )

sync_robot_parts() reads the state from the topics the robot class declares.
discover_interfaces_from_controller_manager() takes the name of the controller manager including its namespace, for example left_arm/controller_manager.
whitelist names the controllers Giskard may use. Tracy lists only its velocity controller, so the joint states are not read a second time. Without a whitelist Giskard uses every active controller of the manager, including a velocity controller that belongs to a gripper. A listed controller that is not loaded or not active stops Giskard at startup instead of leaving its joints uncontrolled.

The world config of such a robot only names the robot class:

@dataclass
class WorldWithTracyConfig(WorldWithFixedRobot):
    """
    A world containing only Tracy, whose base is fixed to the world root.
    """

    root_name: PrefixedName = field(default=PrefixedName("map2"))
    """
    Name of the body Tracy is attached to.
    """

    robot_class: type[Tracy] = field(kw_only=True, default=Tracy, init=False)
    """
    The robot class that is applied to the parsed description.
    """

With a robot class Giskard avoids collisions and uses the velocity limits the class declares.

Robots with a mobile base#

A robot that drives uses WorldWithMobileRobot instead of WorldWithFixedRobot. It adds an odometry body below the root of the world and connects the robot to it with a drive.

Field	What it decides
`drive_class`	`OmniDrive` (the default) or `DifferentialDrive`.
`root_name`, `odometry_body_name`	The names of the root and the odometry body, `map` and `odom` by default.
`translation_velocity_limit`, `rotation_velocity_limit`	The velocity limits of the base.

The interface then says where the pose of the base is read from and where its velocity is sent. The HSR does it like this:

@dataclass
class HSRVelocityInterface(ClosedLoopRobotInterfaceConfig):
    """
    Commands the arm, head and drive of the HSR through their velocity controllers.
    """

    def setup(self):
        self.sync_6dof_joint_with_tf_frame(
            joint=self.world.get_connections_by_type(Connection6DoF)[0],
            tf_parent_frame="map",
            tf_child_frame="odom",
        )

        self.sync_robot_parts()

        omni_drive = self.world.get_connections_by_type(OmniDrive)[0]
        self.add_drive_velocity_command_topic(
            command_topic="/omni_base_controller/cmd_vel", drive=omni_drive
        )

        joints_left = [
            HSRBJoint.ARM_FLEX,
            HSRBJoint.ARM_LIFT,
            HSRBJoint.ARM_ROLL,
            HSRBJoint.WRIST_FLEX,
            HSRBJoint.WRIST_ROLL,
            HSRBJoint.HEAD_PAN,
            HSRBJoint.HEAD_TILT,
        ]
        self.add_joint_group_velocity_command_topic(
            command_topic="/realtime_body_controller_real/command",
            connections=joints_left,
        )

sync_6dof_joint_with_tf_frame() reads the localization, the pose of the odometry body in the root, from tf.
sync_robot_parts() reads the joint states and the odometry from the topics the robot class declares. A robot without a robot class uses sync_joint_state_topic() and sync_odometry_topic() instead.
add_drive_velocity_command_topic() sends the velocity of the drive, as a Twist by default.
add_joint_group_velocity_command_topic() sends joint velocities to a controller that is not found through a controller manager. The joints have to be listed in the order of the controller’s command layout, because the command is an array without names.

Inputs are applied in the order setup() adds them, except the tf frames, which are always applied first.

Where the robot description comes from#

On a real robot, Giskard reads the description from /robot_description with get_robot_description(), so it uses the same model as the robot. It waits until the description is published.
In standalone mode, leave out urdf. The world config then loads the file its robot class declares.

Running it#

Start the robot on mock hardware, with the velocity controller active:

ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=0.0.0.0 \
    use_mock_hardware:=true initial_joint_controller:=forward_velocity_controller

Start Giskard in a second terminal. The script is an ordinary Python program:

python giskardpy/src/giskardpy/middleware/ros2/scripts/tutorial/universal_robot_velocity.py

Giskard reports the controller it connected to and then waits for goals:

Created publisher for /forward_velocity_controller/commands for ['shoulder_pan_joint', ...]
giskard is ready

If Giskard cannot set the robot up, for example because the controller manager is not running, it exits with the error instead of waiting for goals.

Send a joint goal from a third terminal:

python giskardpy/src/giskardpy/middleware/ros2/scripts/tutorial/joint_goal_client.py

#!/usr/bin/env python
from cramph.statechart import Statechart
from semantic_digital_twin.adapters.ros import rospy
from giskardpy.middleware.ros2.giskard_client_executor import GiskardClientExecutor
from giskardpy.motion_statechart.graph_node import EndMotion
from giskardpy.motion_statechart.tasks.joint_tasks import JointPositionList, JointState
from semantic_digital_twin.datastructures.definitions import JointName


class UniversalRobotJoint(JointName):
    """
    The joints of a Universal Robots arm, in the order its velocity controller expects
    its commands.
    """

    SHOULDER_PAN = "shoulder_pan_joint"
    SHOULDER_LIFT = "shoulder_lift_joint"
    ELBOW = "elbow_joint"
    WRIST_1 = "wrist_1_joint"
    WRIST_2 = "wrist_2_joint"
    WRIST_3 = "wrist_3_joint"


def main() -> None:
    """
    Move two joints of the arm to a goal and wait until the motion ends.
    """
    rospy.init_node("giskard_client")
    giskard = GiskardClientExecutor(node_handle=rospy.get_node())
    goal_positions_in_radians = {
        UniversalRobotJoint.SHOULDER_PAN: 0.5,
        UniversalRobotJoint.ELBOW: 1.0,
    }

    motion_statechart = Statechart(context=giskard.context)
    motion_statechart.add_node(
        joint_goal := JointPositionList(
            goal_state=JointState.from_str_dict(
                goal_positions_in_radians, giskard.world
            )
        )
    )
    motion_statechart.add_node(EndMotion.when_true(joint_goal))
    giskard.compile(motion_statechart)
    giskard.execute()

    rospy.shutdown()


if __name__ == "__main__":
    main()

The arm moves in RViz, and ros2 topic echo /joint_states shows the two joints at their goal positions.

Starting Giskard from a launch file#

Running the script with python is enough to get started. To start Giskard together with the rest of a robot, register the main function of the script as a console_scripts entry point of a ROS 2 package and start that executable from a launch file of the package. The giskardpy_ros package in cram_ros2_packages does this for the robots it supports.

Using a real arm#

Start the driver with the address of the arm instead of use_mock_hardware:=true. The Giskard configuration stays the same.

Warning

The robot in this tutorial is described only by its URDF. Giskard therefore does not avoid collisions and limits every joint to a default velocity. Test motions on mock hardware first.