32. Robotics

AGX dynamics offers flexibility in simulating robotics for various tasks and seamless integration with other software. Robots based on CAD drawings or URDF can be imported and configured in Python or C++. Once the model is in place, AGX supports motion planning, testing, developing, and training controllers through reinforcement learning. The controller can live directly in AGX or elsewhere and communicate with the simulation using middleware such as ROS2.

32.1. Serial chains

Although AGX dynamics supports modelling all types of robot mechanisms, the most convenient way to do conventional robotics is to use a agxModel::SerialKinematicChain. It offers functionality for planning and control, including forward, inverse, and differential kinematics, as well as forward and inverse dynamics.

A kinematic chain is a description of a mechanical system. The chain links are rigid bodies that are constrained to each other. Depending on its topology, a chain is classified as either serial, tree, or graph structure. Serial chains and trees are open without loops, whereas the graph can be closed. A serial kinematic chain describes many mechanisms, such as industrial robots, excavator arms, and cranes.

The class agxModel::SerialKinematicChain in AGX represents an open serial chain. There are two ways to initialize this class. The first is to provide a simulation along with the base and end bodies, in which case the implementation searches through the simulation breadth first among the bodies and constraints. The other way to create a chain is to provide a list of bodies and a list of constraints. In this case, the items’ order in both lists is crucial to create a valid chain as it requires that constraint[i] connects body[i] and body[i+1].

32.1.1. Forward kinematics

Forward kinematics is concerned with finding the position and orientation of the end-effector, given the robot’s joint configuration. In the case of serial chains, joint configurations respecting limits result in valid end-effector poses in the task space.

AGX dynamics supports computing forward kinematics relative to the base from a agxModel::SerialKinematicChain. The method computeForwardKinematics computes an AffineMatrix4x4 pose of the end-effector given a joint configuration, including the option to consider joint limits. Similarly, computeForwardKinematicsAll computes the pose of all links in a chain.

Examples of forward computations can be seen in data/python/tutorials/RobotControl/tutorial_set_robot_pose.agxPy.

32.1.2. Manipulator Jacobian

The manipulator Jacobian \(J(q)\) is a component of differential kinematics and essential for controlling and planning the motion of robotic manipulators. It relates the joint velocities \(\dot{q}\) with the velocity of the end-effector \(v\) according to

\[v = J(q) \dot{q}.\]

The method computeManipulatorJacobian evaluates the manipulator Jacobian in fixed frame coordinates relative to the base. It takes the joint positions \(q\) as input and outputs the Jacobian matrix stored by row in a vector with size \(6 \times N_j\).

32.1.3. Inverse kinematics

Inverse kinematics (IK) deals with finding joint configurations that correspond to the desired position and orientation of the end-effector in task space. Unlike forward kinematics, the inverse problem can have zero, multiple, or infinitely many solutions. If the desired pose is unreachable by the manipulator, IK has zero solutions. When a robot has more joints than the dimension of the task space, it is called redundant. While multiple solutions may exist for nonredundant manipulators, e.g. in the case of elbow up or elbow down, only redundant robots can have infinitely many solutions.

A general approach to solve the IK problem for open chains is to rely on iterative numerical algorithms. These take advantage of the inverse of the manipulator Jacobian to find a solution when one exists.

The agxModel:SerialKinematicChain can compute IK via the method computeInverseKinematics. The input is a desired end-effector transform in task space relative to the base, current joint positions to help guide the algorithm toward a solution, and an optional solver tolerance.

We recommend checking the IK computations return status (enum) before using the output joint angles. Also, the best practice is to use the latest joint positions as an initial guess. This is to avoid cases where a slight change in end-effector transform causes an element in the output joint position to switch from \(\pi\) to \(-\pi\).

32.1.4. Inverse dynamics

Dynamics is the study of motions and the forces and torques that cause them. AGX typically follows this process: Advancing the simulation based on current forces and torques acting on bodies and particles, resulting in a new state with updated positions and velocities.

The problem of inverse dynamics (ID) is to find the forces/torques that result in a certain state. This is the opposite of forward dynamics, where the forces and torques are known, but the new state is not.

Unlike kinematics computations, inverse dynamics computations in the SerialKinematicChain require knowledge of the simulation. Therefore, the chain must receive a simulation instance either through the constructor or by invoking setSimulation at a later stage.

With knowledge of the simulation, computeInverseDynamics performs the ID computations with an input corresponding to the desired joint accelerations, \(\ddot{q}_\mathrm{desired}\). The rest of the state, current \(q\) and \(\dot{q}\) are available via the simulation. The computed output is a vector with forces/torques \(\tau\). One overload to computeInverseDynamics also accepts additional external forces acting on the chain’s links.

Examples with ID including external forces are available both in python data/python/tutorials/RobotControl/tutorial_pick_and_place.agxPy and C++ tutorials/agxOSG/tutorial_robot_pick_and_place.cpp.

../_images/tutorial_pick_and_place.png

Ensure thread safety when using computeInverseDynamics. The method reads data from the chain’s components and transforms constraint attachment frames. Therefore, calling this method from a separate thread while the solver runs in another may cause incorrect results or undefined behaviour. Instead, recommended use is between calls to Simulation::stepForward or within a StepEventListener callback method, such as pre or preCollide.

32.2. Robot control

How to control a robot to achieve the desired behaviour depends on the working environment and the task tries to solve. AGX Dynamics aims to provide functionality that makes it easy to test, develop, and train controllers in dynamic simulation environments for various applications.

32.2.1. PID control

The most basic controller is the Proportional-Integral-Derivative (PID) controller. As a PID lacks a dynamic model of the robot, control relies entirely on feedback. The following mathematical expression describes it:

\[u(t) = MV(t) = K_p e(t) + K_i \int_{0}^{t} e(\tau) d\tau + K_d \frac{de}{dt},\]

where \(u\) is the output signal, also known as the Manipulated Variable (MV), \(e\) is the error between the measured process variable and the set point, and \(K_p\), \(K_i\), and \(K_d\) are the tuning parameters, or proportional gain, integral gain, and derivative gain.

A PID controller can be used on its own or as part of a controller. For more details about the PID implementation in AGX and examples of PID control, see AGX Model: PID Controller.

32.2.2. Velocity control

Although most robots use actuators with force or torque inputs, these may be set to velocity mode for control. Velocity mode means that the velocities of the joints are controlled directly.

In AGX dynamics, joints are modelled as constraints with a single degree of freedom, i.e. either as a agx::Hinge or agx::Prismatic. These constraints can activate a agx::Motor1D, which allows setting a desired joint velocity through the setSpeed method.

The tutorial data/python/tutorials/RobotControl/tutorial_differential_kinematics.agxPy demonstrates velocity control of the Franka Emika Panda when the task is to track a moving object. In the tutorial, we implement a control method called Position-based servoing, which relies on the pseudoinverse of the manipulator Jacobian to compute joint velocities.

../_images/tutorial_differential_kinematics.png

32.2.3. Torque control

In contrast to calculating torques using a PID controller, precise torque control requires a dynamics model of the robot. When a dynamics model is at hand, inverse dynamics calculates the torques that achieve a desired motion. For instance, the tutorial data/python/tutorials/RobotControl/tutorial_gravity_compensation.agxPy demonstrates how to counteract gravity and maintain zero acceleration.

However, as no model is perfect, most practical controllers rely on feedback. A motion control algorithm that combines feed-forward with feedback is computed torque control as demonstrated in data/python/tutorials/RobotControl/tutorial_computed_torque_control.agxPy.

../_images/tutorial_computed_torque_control.png

Regardless of where the desired torques come from, a simple way to torque-control a robot in AGX dynamics is to use a agx::Motor1D on each joint. An agx::Motor1D achieves the desired torque tau by setting the same value to the upper and lower limit through setForceRange(tau, tau). For more accurate modelling of the actuator dynamics, refer to AGX DriveTrain. However, agx::DriveTrain is currently not supported by computeInverseDynamics but is expected to be so in the near future.

32.3. Reinforcement Learning with AGX Dynamics

This section shows how to wrap an AGX Dynamics simulation as an OpenAI gym environment, and how to use that environment to train intelligent agents using AGX Dynamics high fidelity physics and popular RL-libraries such as stable-baselines3, pfrl or RLlib <https://docs.ray.io/en/latest/rllib/index.html>.

../_images/RL-loop.png

AGX Dynamics gives you reliable nonsmooth multidomain dynamics simulation. It can run faster than realtime, generating all the accurate data you need for training your agents. And easing the transition from simulations to the real life application.

Install the python requirements with the command pip install -r data/python/RL/requirements.txt and test the included examples with the command python data/python/RL/cartpole.py to run the cartpole example with an untrained policy, or python data/python/RL/cartpole.py --load data/python/RL/policyModels/cartpole_policy.zip to run it with a pre-trained policy.

Note

We do not support platforms with python versions < 3.7.

32.3.1. AGX OpenAI gym environment

An OpenAI gym environment typically contains one RL-agent that can (partially or fully) observe and act upon its surroundings, effectively changing the state of the environment.

Developers of custom environments must implement these main gym environment API methods:

  • step() - Sets the next action on the agent, steps the environment in time and returns the observation, reward, terminal and optional info.

  • reset() - Resets the environment to an initial state and return an initial observation

  • render() - Renders the environment.

  • close() - Environments automatically close themselves when garbage collected or when the program exits

  • seed() - Sets the seed for this env’s random number generator(s)

And the following attributes

  • action_space - The gym.Space object corresponding to valid actions

  • observation_space - The gym.Space object corresponding to valid observations

  • reward_range - A tuple corresponding to the min and max possible rewards

The class AGXGymEnv inherits from the OpenAI gym environment. It initializes AGX, creates an agxSDK::Simulation, implements the main gym API methods and cleanup all the resources on exit. Leaving the user of AGXGymEnv with implementing methods for modeling the scene, settings actions and returning observations.

  • _build_scene() - Builds an AGX Dynamics simulation (models the environment). Much like the BuildScene() function in the python tutorials. This method is called in env.reset().

  • _set_action(action) - Sets the action on the agent. This method is called in env.step()

  • _observe() - returns the tuple (observation->numpy.array, reward->float, terminal->bool, truncated->bool, info->dict). This method is called in env.step().

If you would like to have a rendered image as an observation you can choose to initialize graphics and use agxOSG::ExampleApplication for some simple rendering. Rendering is controlled by the user.

env = CartpoleEnv(render_mode="rgb_array", headless=True)

env.reset()
for _ in range(10):
  env.step(env.action_space.sample())

The render_mode can be "human" or rgb_array, additionally the argument headless controls if a Window for displaying the graphics will be created or not. Calling image = env.render() will return the first image from the virtual_cameras in the environment. To create virtual cameras you must implement the method AGXGymEnv._setup_virtual_cameras(). Checkout the agxPythonModules.agxGym.envs.cartpole.CartpoleEnv for an example on how to do this.

32.3.2. Example environments

You can start any of the example environments by running python data/python/RL/run_env.py --env name-of-environment. That will start the environment and control the agent using a random policy. To list the available environments run python data/python/RL/run_env.py -l.

32.3.2.1. Cartpole environment

Run the example with python data/python/RL/cartpole.py. Add the argument --train to train a new policy model. Add the argument --load path/to/trained/policy to load a previously trained policy, either to continue training it or just to demo the results.

The cartpole environment is an example on the classical environment with a cart allowed to move in one dimension. A pole rotating around one axis is attached to the cart. By moving the cart correctly it is possible to balance the pole. This is a example of how to implement cartpole as an AGXGymEnv environment. The observations is the position of the cart, the rotation of the pole, the velocity of the cart and the angular velocity of the pole. And the action is the force applied to the cart at each timestep.

To run the same example but with only a camera as observation, just add the argument --observation-space visual. That will initialize graphics during training, and use a small convolutional network as feature extractor.

32.3.2.2. Pushing robot environment

Run the example with python data/python/RL/pushing_robot.py. Add the argument --train to train a new policy. Add the argument --load path/to/trained/policy to load a previously trained policy, either to continue training it or just to demo the results.

The pushing robot environment is an example of how to create a bit more complicated environment. It is a robot with two free degrees of freedom that must find the box in front of it and use its end-effector to push it away from the robot. The observations are the angle and speed of the robot joints, the world position of the tool and the relative position between tool and box. The action is what torque to apply to each joint at each timestep. The reward is distance between the current position of the box and its starting position.

32.3.2.3. Wheel loader terrain environments

We have several different environments with all the different wheel loader models that comes with AGX Dynamics. The names of the environments depends on the wheel loader model. All the wheel loader environments are listed below.

  • agx-wa475-terrain-v0

  • agx-wa475-gravel-pile-and-rocks-v0

  • agx-wa475-flat-terrain-and-rocks-v0

  • agx-dl300-terrain-v0

  • agx-l70-terrain-v0

  • agx-algoryxwheelloader-terrain-v0

You can run any of the environments with a random action policy using the command python data/python/RL/run_env.py --env env-name. Add the argument --policy keyboard or --policy heuristic to control the wheel loader using the keyboard or a pre-programmed heuristic policy.

32.3.2.4. Excavator terrain environments

Run the excavator 365 environment with the command python data\python\RL\run_env.py --env agx-365-terrain-v0. Add the argument --policy keyboard or --policy heuristic to control the wheel loader using the keyboard or a pre-programmed heuristic policy.

32.4. ROS2

The Robot Operating System (ROS) is a middleware and framework that is often used to work with robotics. It is not an Operating System even if the name implies so. One fundamental part in ROS2 is message-passing between different nodes, for example a controller that could be sending and receiveing messages for a real or virtual robot.

ROS2 support in AGX Dynamics comes in two levels:

  1. AGX Dynamics built in ROS2 support (see Built in ROS2 support for details).

  • Always available in AGX Dynamics on all supported operating systems except Ubuntu 18.04.

  • Publisher / Subscriber message passing support.

  • Supports all message types in std_msgs, sensor_msgs, builtin_interfaces, rosgraph_msgs, geometry_msgs and agx_msgs.

  • Mechanism for serialization / deserialization of custom types (see Using custom types for details).

  • QOS and domain ID support.

  • No user-defined ROS2 package compilation support.

  1. Pre-built ROS2 installation (see Pre-built ROS2 installation for details).

  • Available for Windows only.

  • Can be used together with AGX Dynamics in Python environments.

  • User defined ROS2 packages can be compiled and used.

  • Several built in ROS2 features that come with a typical ROS2 installation.

32.4.1. Built in ROS2 support

With AGX Dynamics it is possible to do Publisher / Subscribe message passing. It does not require a separate ROS2 installation or build in order to work.

All messages types in the following packages are supported:

std_msgs, sensor_msgs, builtin_interfaces, rosgraph_msgs, geometry_msgs and agx_msgs.

AGX Dynamics offers an API through the agxROS2 namespace that wraps some of the ROS2 functionality, namely Publisher and Subscriber message passing.

Specifying QOS such as reliability, durability, history type and depth as well as domain ID is also supported.

It targets the version ROS2 Humble and specifically the DDS implementation Fast-DDS which is the default DDS implementation in ROS2 Humble. ROS2 generally does not guarantee compatibility between ROS2 versions and DDS vendors, but it should work in practice, especially when matching the DDS vendor types. For more details regarding specifying the DDS vendor in a ROS2 environment, see Troubleshooting.

The built in ROS2 support does not support compiling and using user defined message types, but there is a mechanism avaiable to enable sending and receiving custom data types, see Using custom types for details.

Below are two minimal examples of how to create a Publisher and Subscriber through the built in ROS2 support and passing a message between them. Note that the Publisher and Subscriber code normally are run on two separate applications and/or computers.

In C++:

#include <agxROS2/Publisher.h>
#include <agxROS2/Subscriber.h>
#include <iostream>

int main()
{
  // Create Publisher and Subscriber with specific Topic.
  agxROS2::stdMsgs::PublisherFloat32 pub("my_topic");
  agxROS2::stdMsgs::SubscriberFloat32 sub("my_topic");

  // Create a message and send it.
  agxROS2::stdMsgs::Float32 msgSend;
  msgSend.data = 3.141592f;
  pub.sendMessage(msgSend);

  // Try to receive the message.
  agxROS2::stdMsgs::Float32 msgReceive;
  if (sub.receiveMessage(msgReceive))
    std::cout << "Received message: " << msgReceive.data << ".\n";
  else
    std::cout << "Did not receive a message.\n";

  return 0;
}

In Python:

import agxROS2

def main():
    # Create Publisher and Subscriber with specific Topic.
    pub = agxROS2.PublisherStdMsgsFloat32("my_topic")
    sub = agxROS2.SubscriberStdMsgsFloat32("my_topic")

    # Create a message and send it.
    msgSend = agxROS2.StdMsgsFloat32()
    msgSend.data = 3.141592
    pub.sendMessage(msgSend)

    # Try to receive the message.
    msgReceive = agxROS2.StdMsgsFloat32()
    if sub.receiveMessage(msgReceive):
        print("Received message: {}".format(msgReceive.data))
    else:
        print("Did not receive a message.")

if __name__ == '__main__':
    main()

More advanced usage such as using a specific QOS or custom types can be seen in the tutorial tutorials/agxOSG/tutorial_ros2.cpp.

32.4.1.1. ros2_control interface

ros2_control is a commonly used framework to control robots using ROS2. You can find the documentation to ros2_control (here). We provide an easy way to communicate with ros2_control controllers using class ROS2ControlInterface which inherits from agxSDK::StepEventListener. It assumes that the joint commands and states is communicated on ROS2 topics using the sensor_msgs/msg/JointState message type. And that the robot loaded into ros2_control uses the (topic_based_ros2_control) hardware interface configured to use the same topics.

// Read your robot urdf file
agxSDK::AssemblyRef model = agxModel::UrdfReader::read(urdfFile, urdfPackage, initialPositions.get(), settings);

// Create the ROS2ControlInterface StepEventListener with set ROS2 topics for commands and states
auto ros2ControlInterface = agxROS2::ROS2ControlInterface("joint_commands", "joint_states");

// Add the 1DOF contraints from the robot you want to control and how you want to control it
ros2ControlInterface.addJoint(model->getConstraint1DOF("joint1"), agxROS2::ROS2ControlInterface::POSITION)
ros2ControlInterface.addJoint(model->getConstraint1DOF("joint2"), agxROS2::ROS2ControlInterface::POSITION)

// Add the listener to the simulation
simulation->add(ros2ControlInterface)

Or in python with

# Create an excavator
  excavator = Excavator()

  excavator.setRotation(terrain.getRotation())
  excavator.setPosition(0, 0, 0)
  simulation().add(excavator)

  joint_names = [
      'ArticulatedArm_Prismatic',
      'ArmPrismatic',
      'StickPrismatic',
      'TiltPrismatic',
      'BucketPrismatic',
      'BladePrismatic1',
      'BladePrismatic2',
      'CabinHinge',
      'LeftSprocketHinge',
      'RightSprocketHinge',
  ]

  excavator_ros2_interface = agxROS2.ROS2ControlInterface(
      "joint_commands",
      "joint_states",
  )
  for name in joint_names:
      excavator_ros2_interface.addJoint(excavator.getConstraint1DOF(name), agxROS2.ROS2ControlInterface.VELOCITY)

32.4.1.2. Troubleshooting

If messages are missed, or the first couple of messages are missed, it may be of use to check the QOS settings used. By default the QOS settings are the same as in ROS2, i.e. RELIABILITY “reliable”, DURABILITY “volatile”, HISTORY “keep last” and history depth 10. More details on these settings can be seen (here).

If no communication between a built in agxROS2::Publisher and a built in agxROS2::Subscriber can be established it is possible that it is blocked by a firewall on the computer. Check the firewall settings and unblock the process in case it has been blocked.

If no communication between a built in agxROS2::Publisher/Subscriber and an external ROS2 node running on a complete ROS2 installation can be established, ensure that the same version of ROS2 is used. The built in ROS2 support in AGX Dynamics targets ROS2 Humble. If this does not solve the problem, ensure that the DDS implementation Fast-DDS is used in the ROS2 installation. This can be forced by setting the environment variable RMW_IMPLEMENTATION=rmw_fastrtps_cpp. See the (ROS2 documentation) for details. Also check the firewall settings if necessary.

If the creation of a agxROS2::Publisher or agxROS2::Subscriber hangs forever it is possible a seemingly rare bug in Fast-DDS is the culprit. This seems to happen when creating a DDS Participant (which is done automatically by AGX Dynamcis) on Windows on rare occations. The underlying reason why this can happen is not known, but a manual workaround is to do the following: 1. Turn off any process using ROS2. 2. Manually remove all files in C:\ProgramData\eprosima\fastrtps_interprocess (replace drive-letter if applicable).

32.4.1.3. Using custom types

There is some built in support for sending custom data types via ROS2. This is done using the AnyMessageBuilder and AnyMessageParser. The AnyMessageBuilder can serialize and deserialize several common built in primitive types to an agxMsgs::Any message. This message can then be sent on any Topic, received and deserialized using the AnyMessageParser.

The agx_msgs::Any message type as well as AnyMessageBuilder and AnyMessageParser are also avaiable open-source on GitHub so that these can be used on machines that does not have AGX Dynamics or installed.

Below is a small example of how this can be used.

#include <agxROS2/AnyMessageBuilder.h>
#include <agxROS2/AnyMessageParser.h>
#include <agxROS2/Publisher.h>
#include <agxROS2/Subscriber.h>

struct MyStruct
{
  std::string myString;
  std::vector<int64_t> myInts;
  float myFloat{ 0.f };
};

int main()
{
  // Custom struct that we will send via ROS2.
  MyStruct sendStruct;
  sendStruct.myString = "We are all star stuff";
  sendStruct.myInts = { 42, 13, -999 };
  sendStruct.myFloat = 2.71828f;

  // Create Publisher and Subscriber with specific Topic.
  agxROS2::agxMsgs::PublisherAny pub("any_msg");
  agxROS2::agxMsgs::SubscriberAny sub("any_msg");

  // Serialize the custom struct type.
  agxROS2::AnyMessageBuilder builder;
  builder
    .beginMessage()
    .writeString(sendStruct.myString)
    .writeInt64Sequence(sendStruct.myInts)
    .writeFloat32(sendStruct.myFloat);

  // Send the message.
  pub.sendMessage(builder.getMessage());

  // Try to receive the message.
  agxROS2::agxMsgs::Any msgReceive;
  if (!sub.receiveMessage(msgReceive))
  {
    std::cout << "Dit not receive a message.\n";
    return 1;
  }

  // De-serialize the message and read back the custom struct that was received.
  MyStruct receiveStruct;
  agxROS2::AnyMessageParser parser;
  parser.beginParse();
  receiveStruct.myString = parser.readString(msgReceive);
  receiveStruct.myInts = parser.readInt64Sequence(msgReceive);
  receiveStruct.myFloat = parser.readFloat32(msgReceive);

  // receiveStruct now contains the data that was sent by the publisher.

  return 0;
}

Another example of this is available in the tutorial tutorials/agxOSG/tutorial_ros2.cpp.

32.4.2. Pre-built ROS2 installation

The pre-built ROS2 installation is available for Windows only. It can be installed through the AGX Dynamics installer. At the end of the AGX Dynamics installation, there is a checkbox to download and unzip a pre-built ROS2. It can also be downloaded manually from (here).

Un-zip it and place it in C:\opt such that the path to local_setup.bat is C:\opt\ros2-algoryx\install\local_setup.bat. The placement of the ROS2 installation must match exactly this.

The pre-built ROS2 installation is of version ROS2 Humble.

To be able to build custom ROS2 packages, CMake and Visual Studio must be installed manually on the computer.

There is no specific AGX Dynamcis integration when using this alternative, but it is possible to use AGX Dynamics and a ROS2 installation in the same Python environment. Here, a simple ROS2 Publisher class and a ROS2 Subscriber class is added to an AGX Python script.

# Include the necessary ROS2 modules.
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

# ROS2 publisher class.
class RosPublisher(Node):
  def __init__(self, node_name, topic_name):
    super().__init__(node_name)

    self.publisher = self.create_publisher(Float32, topic_name, 1)

  # Publish a ROS2 message.
  def send_data(self, data):
    msg = Float32()
    msg.data = data
    self.publisher.publish(msg)

# ROS2 subscriber class.
class RosSubscriber(Node):
  def __init__(self, node_name):
    super().__init__(node_name)

  # Note: the callback function must have an input parameter for the message.
  def register_callback(self, topic, callback_function):
    self.create_subscription(
      Float32,
      topic,
      callback_function,
      1)

32.5. URDF

AGX Dynamics provides a URDF reader that takes a URDF file and returns an AGX Dynamics representation of the model.

32.5.1. UrdfReader class

The agxModel::UrdfReader class has a static read method for reading urdf files and returning an agxSDK::Assembly object with all the robot parts (bodies, geometries and constraints). The behavior of the read method can be controlled using the agxModel::UrdfReader::Settings class. See the examples below for more information.

32.5.2. Examples

Below are two simple examples of how to read a URDF model and access one of its joints, both in C++ and Python. Also, instructions for running the URDF Panda ROS2 Python example is described below.

32.5.2.1. C++ example

Read a URDF model and access one of its joints.

// Absolute path to the URDF file.
agx::String urdfFile = "absolute/path/model_package/description/model.urdf";

// (Optional) Absolute path to the URDF package path pointing to the 'package://'
// part of any filename attribute in the URDF file if existing.
agx::String urdfPackagePath = "absolute/path/model_package";

// (Optional) Initial joint positions. Must match in length the total number of
// revolute, continous and prismatic joints in the model.
agx::RealVector initJointPos {agx::PI/2.0, -agx::PI/4.0, 0.0, 0.0, 0.0, 0.0};

agxModel::UrdfReader::Settings settings;

// Determines if the base link of the URDF model should be attached to the
// world or not.
// Default is false.
settings.attachToWorld = true;

// If true the collision between linked/constrained bodies will be disabled.
// Default is false.
settings.disableLinkedBodies = true;

// If true, any link missing the <inertial></inertial> element will recieve
// a negnegligible mass and will be treated as a kinematic link
// and will be merged with its parent. According to http://wiki.ros.org/urdf/XML/link
// If false, the body will get its mass properties calculated from the shape/volume/density.
// Default is True.
settings.mergeKinematicLink = true;

// Read the URDF.
agxSDK::AssemblyRef model =
  agxModel::UrdfReader::read(urdfFile,
                             urdfPackagePath,
                             &initJointPos,
                             settings);

// Add the model to the agxSDK::Simulation.
simulation->add(model);

// Access one of the joints in the model and set wanted speed for it. The name of the constraint
// corresponds to the name of the joint in the URDF file.
agx::Constraint1DOF* joint1 = model->getConstraint1DOF("joint_name");
joint1->getMotor1D()->setEnable(true);
joint1->getMotor1D()->setSpeed(0.5);

32.5.2.2. Python example

Read a URDF model and access one of its joints.

# Absolute path to the URDF file.
urdfFile = "absolute/path/model_package/description/model.urdf"

# (Optional) Absolute path to the URDF package path pointing to the 'package://'
# part of any filename attribute in the URDF file if existing.
urdfPackagePath = "absolute/path/model_package"

# (Optional) Initial joint positions. Must match in length the total number of
# revolute, continous and prismatic joints in the model.
initJointPos = agx.RealVector()
initJointPos.append(agx.PI/2.0)
initJointPos.append(-agx.PI/4.0)
for i in range(0, 4):
  initJointPos.append(0.0)

# Determines if the base link of the URDF model should be fixed to the
# world or not. Default is false.
fixToWorld = True

# If true the collision between linked/constrained bodies will be disabled.
# Default is false.
disableLinkedBodies = True

# If true, any link missing the <inertial></inertial> key will be treated
# as a kinematic link and will be merged with its parent. According to http://wiki.ros.org/urdf/XML/link
# If false, the body will get its mass properties calculated from the shape/volume/density.
# Default is True.
mergeKinematicLink = True

settings = agxModel.UrdfReaderSettings(fixToWorld, disableLinkedBodies, mergeKinematicLink)

# Read the URDF.
model = agxModel.UrdfReader.read(urdfFile, urdfPackagePath, initJointPos, settings)

# Add the model to the agxSDK.Simulation. Notice the .get() which is needed since model
# is an agxSDK.AssemblyRef.
sim.add(model.get())

# Access one of the joints in the model and set wanted speed for it. The name of the constraint
# corresponds to the name of the joint in the URDF file.
joint1 = model.getConstraint1DOF("joint_name")
joint1.getMotor1D().setEnable(True)
joint1.getMotor1D().setSpeed(0.5)

32.5.2.3. ROS2 URDF Panda example

Start the URDF Panda robot simulation

Simply double-click the urdf_panda_ros2.agxPy located in Algoryx/AGX-version/data/python/ROS2.

Start the URDF Panda ROS2 controller

  1. Open a new “AGX Dynamics Command Line”.

  2. In the AGX command prompt, cd to the AGX ROS2 examples directory, e.g:

cd Algoryx/AGX-version/data/python/ROS2

  1. Run the URDF Panda ROS2 controller script by calling python, i.e:

python urdf_panda_ros2_controller.py

32.5.3. Supported features overview

Below is an overview of the URDF features. Those marked with ‘✓’ are supported by the URDF reader and those marked with ‘☓’ are not.

robot ✓
│   name ✓
│
└───link ✓
│   │   name ✓
│   │
│   └───inertial ✓
│   │   │
│   │   └───origin ✓
│   │   │      xyz ✓
│   │   │      rpy ✓
│   │   │
│   │   └───mass ✓
│   │   │      value ✓
│   │   │
│   │   └───intertia ✓
│   │          ixx - izz ✓
│   │
│   └───visual ✓
│   │   │   name ✓
│   │   │
│   │   └───origin ✓
│   │   │      xyz ✓
│   │   │      rpy ✓
│   │   │
│   │   └───geometry ✓
│   │   │   │
│   │   │   └───box ✓
│   │   │   │      size ✓
│   │   │   │
│   │   │   └───cylinder ✓
│   │   │   │      radius ✓
│   │   │   │      length ✓
│   │   │   │
│   │   │   └───sphere ✓
│   │   │   │      radius ✓
│   │   │   │
│   │   │   └───mesh ✓
│   │   │          filename ✓
│   │   │          scale ✓
│   │   │
│   │   └───material ✓
│   │       │   name ✓
│   │       │
│   │       └───color ✓
│   │       │      rgba ✓
│   │       │
│   │       └───texture ✓
│   │
│   └───collision ✓
│       │   name ✓
│       │
│       └───origin ✓
│       │      xyz ✓
│       │      rpy ✓
│       │
│       └───geometry ✓
│              (see geometry under 'visual' above)
│
└───joint ✓
│   │   name ✓
│   │   type: revolute ✓
│   │   type: continuous ✓
│   │   type: prismatic ✓
│   │   type: fixed ✓
│   │   type: floating ✓
│   │   type: planar ✓
│   │
│   └───parent ✓
│   │      link ✓
│   │
│   └───child ✓
│   │      link ✓
│   │
│   └───axis ✓
│   │      xyz ✓
│   │
│   └───calibration ☓
│   │
│   └───dynamics ✓
│   │      damping ☓
│   │      friction ✓
│   │
│   └───limit ✓
│   │      lower ✓
│   │      upper ✓
│   │      effort ✓
│   │      velocity ☓
│   │
│   └───mimic ✓
│   │      multiplier ✓
│   │      offset ✓
│   │
│   └───safety_controller ☓
│
└───transmission ☓
│
└───gazebo ☓
│
└───sensor ☓
│
└───model_state ☓

32.5.4. AGX Dynamics representation

Below is a detailed description of the how the URDF features are represented by AGX Dynamics.

32.5.4.1. <robot>

The root element robot is represented by an agxSDK::Assembly. The name of the agxSDK::Assembly is set to the required name attribute.

32.5.4.3. <transmission>

Not supported and will be ignored.

32.5.4.4. <gazebo>

Not supported and will be ignored.

32.5.4.5. <sensor>

Not supported and will be ignored.

32.5.4.6. <model_state>

Not supported and will be ignored.