29. Robotics

This chapter is a collection of topics relevant for robotics and control.

29.1. URDF

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

29.1.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.

29.1.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.

29.1.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);

29.1.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)

29.1.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

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 ☓

29.1.3. AGX Dynamics representation

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

29.1.3.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.

29.1.3.3. <transmission>

Not supported and will be ignored.

29.1.3.4. <gazebo>

Not supported and will be ignored.

29.1.3.5. <sensor>

Not supported and will be ignored.

29.1.3.6. <model_state>

Not supported and will be ignored.

29.2. ROS2

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.

29.2.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.

29.2.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)

29.2.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).

29.2.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.

29.2.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)

29.3. Inverse Dynamics

AGX Dynamics is computing forward dynamics. For the system to end up in a specific desired state there has to exist some kind of control loop that makes sure the system converge toward a specific state. The agxModel::InverseDynamics class offer a solution for a user of AGX Dynamics to choose a sub-set of the simulation to be pre computed, making sure a certain state is reached at a certain time.

Imagine for example a multi axis robot moving its end effector to a specific point in space, or following a specific straight line. The Inverse Dynamics will pre compute the torques and forces needed to achieve this motion. With the limitation that the system is isolated from interacting with other things before reaching the end state. If however a contact or other interaction happens, the API provides partial results so re-planning of trajectories can be performed.

The Inverse Dynamics class will find one possible solution for integrating the forward dynamics system to a specific state for one of the included rigid bodies. All other rigid bodies included in the isolated sub system will be integrated according to the degrees of freedom available. The rigid bodies of the sub system will obey the constraints included. If a certain path out of many different ones is desired from the start pose to the end pose, then a solve-method that takes multiple delta times and transforms as input should be used, or the path between startpose and endpose split up in several solve calls.

For example a robot arm may end up in an unwanted state, where one or more joints are near a range limit, which could result in jerky motion. Then it can be adventagous to use a strategy where not just startpose and endpose are used as inputs to Inverse Dynamics.

29.3.1. The InverseDynamics class

The class agxModel::InverseDynamics can be used to perform Inverse Dynamics computations.

When an instance of InverseDynamics is created the inputs are cloned and an internal representation is made. This is so that the original simulation is not affected by the computations and that the output then can be applied to the original system.

Two sets of bodies/constraints are maintained internally. One set is used for trajectory computations. These bodies are related to inverse kinematics. The other set is used for force/torque computations.

To analyze the state in the internal, cloned systems, their representation can be accessed by either

const agxSDK::Assembly* internalMotionData = id->getMotionData();

to get the final state of the system used for calculating the trajectories. Or

const agxSDK::Assembly* internalDynamicsData = id->getDynamicsData();

to get the final state of the system used for calculating forces and torques required to reach the desired transform.

The steps to use InverseDynamics are:

  • Specify bodies/constraints that should be included in the computations.

  • The inputs are mirrored when the InverseDynamics constructor is called.

  • Use sync if needed to make sure that the ID internal bodies have matching state as the source bodies.

  • Call solve with a desired transform for a body that is part of the inputs and with a delta time when the body should be at that transform.

  • The amount of produced data depends on simulation timestep and delta time.

  • Fetch results from ID via getJointForces by specifying which simulation frame starting from 0. The output is a view into an array with structs that contains joint index, angle index and the actual force/torque value.

An example of this is available in build_inverse_dynamics_scene in python/tutorials/tutorial_InverseDynamics.agxPy.

Note

The sync method should not be called at the same time the source simulation is being stepped.

29.3.2. The different solve methods

The InverseDynamics has different versions of the solve method. All tries to get to the target pose. The difference is the intermediate path, the number of possible constraints that are connected to an equal number of kinematic bodies and whether the solve is pre calculated, or calculated on the fly.

The method solveLinear tries to follow a linear path from the current pose towards the target pose. If for example a straight edge should be followed and it is all within reach, then this method generally works well. If part of the path is beyond reach, the path will not be straight. Studying the angle for a single joint, it can move both in positive and negative direction on its way from start pose to end pose.

../_images/id-solvelinear.png

The solve method only uses the initial pose and the end pose and all angles inbetween are interpolated. With this method each joint angle will monotonically change from start angle to end angle. This results in smoother movements, less force/torque spikes but a different trajectory. The downside is instead that an edge of a box can not be traced by doing one solve call since the paths that are followed are generally more curved.

Which to use depends on desired result. Overexagerating the difference, the robot to the left uses solveLinear and the robot to the right uses solve. Part of the trajectory was problematic to reach with the specified orientation for the end effector.

../_images/id-solve-comparison.png

There are two additional versions of the solveLinear method.

solveLinearN takes a list of MoveOperation’s where each individual move will generate a kinematic trajectory body, which is constrained to a body in the motion simulation. Each move can have an individual threshold for both rotation and translation, to be considered successful. Also the stiffness of the constraint connecting the kinematic trajectory body to the chosen body can be tuned, separating rotational and translational stiffness, to give the stiffer trajectory constraints the upper hand.

There is an adaptive solve linear method that will sync the dynamics simulation state with the source simulation each frame. The adaptive will not calculate any trajectory in advance, but will require more computations during runtime. The purpose is to give a planner the chance to make adjustments to unexpected interactions, and to do continous updates to a manipulation task. initializeAdaptiveSolveLinearN will initialize the adaptive solve with a set of defined moves. It is up to the user to each timestep call stepAdaptiveSolveN to compute new torques and angles. If the planner somehow detects that it need to reconfugure the defined move operations, it can send the same, but reconfugured, operations to the updateAdaptiveSolveN. If a complete new set of move operations are required finalizeAdaptiveSolveN must be called before it can be initialized again.

All solve methods aims at landing on the target transform with zero velocity.

29.3.3. Gravity Compensation

Inverse dynamics can be used to compute gravity compensation for a constrained system. Which forces/torques are needed to maintain current pose with zero velocity?

To use this functionality one creates an InverseDynamics instance. Then at each timestep, a sync is needed and after that the forces/torques needs to be applied to the source simulation before the solver is stepped.

Examples with a robot and gravity compensation is available in python/tutorials/tutorial_InverseDynamics.agxPy. Examples are with and without powerlines.

29.3.4. Inverse dynamics and agxPowerLine

The InverseDynamics class has partial support for agxPowerLines connected to the input constraints.

Upon construction of an InverseDynamics instance, powerlines and units can be included. The requirements are:

  • Each powerlines must have one or more Actuators that act upon constraints that are part of the input to InverseDynamics.

  • Each unit in the unit container must belong to one of the input powerlines.

  • The units must be based upon RotationalUnit. This includes most units but excludes e.g. parts of agxHydraulics.

Output will be generated for the units in the unit container, not all units in the input powerlines.

When the different solve methods are used, the output for the units will be value (position) and gradient (velocity) for each unit as well as the regular force/torque output values for the joints. How the powerline unit values should be used to control the actual joint is highly scene dependent and depends on power source (e.g. ElecticMotor, Engine, …) and use of differentials, clutches etc.

If gravity compensation is used then velocity constraints are used on rotational units to prevent movement and since the powerline components are connected to a joint via an actuator, the constrained bodies will not move. The outputs produced are still returned as value and gradient but only the first one is to be used. Gradient will be set to 0. The value containes the load needed on the unit to maintain zero velocity at the constraint. Example of how this can be used is available in the previously mentioned gravity compensation tutorial scenes.

29.4. PID Controller

In agxModel::PidController1D there is an implementation of a PID controller with the classic gains for proportional, integral, and derivative gain described by the following mathematical expression:

\[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 out signal, also known as the Manipulated Variable (MV), and \(K_p\), \(K_i\), and \(K_d\) are the tuning parameters or proportional gain, integral gain, and derivative gain. In the figure below, Fig. 29.1, there is a schematic representation of the PID controller including the process the PID is controlling, known as the Plant.

The above equation is discretized with the forward Euler Method and this leads to an explicit solution of \(u(t)\). The error, \(e\), is calculated as the difference between the measured Process Variable (PV) on the Plant and the desired Set Point (SP), the derivative of the error is discretized as \(\frac{de}{dt} = \frac{e - e_{n-1}}{t - t_{n-1}}\), where \(e_{n-1}\) is the error from the previous time step and \(t - t_{n-1}\) is the time step. The integral part is approximated by adding up the error, \(e\), multiplied by the time step. This explicit solution of \(u(t)\) is relatively sensitive to high frequencies in the Plant.

The PID controller has an integral windup algorithm with both “clamping” and/or an integral back-calculation algorithm. The clamping is disabled per default and the back-calculation is activated if the PID controller are given constraints on the Manipulated Variable (MV). The backward calculation is done by recomputing the integral term to its previous state before the windup started and by that disabling the integration.

../_images/pid_diagram.png

Fig. 29.1 Above is a schematic diagram over the PID controller. From left to right we have the error which is the difference between the Set Point (SP) and the Process Variable (PV). \(K\) represents the three gains and the block to the right represent the integral windup algorithm activated when the output signal, Manipulated Variable (MV), is outside the Plant input limits.

29.4.1. Usage

It is possible to use the PID controller in a stand alone application but the agxModel::PidController1D is preferable to be used together with the agxModel::ControllerHandler which is a StepEventListener. The agxModel::ControllerHandler reads the Process Variable (PV) from the Plant, or controlled process, and lets the PID controller calculate a new Manipulated Variable (MV) with respect to the current Set Point (SP), and then that value is sent to the Plant.

In the tutorial below we create a pendulum with a load attached via a wire to a winch, see Fig. 29.2. The winch is controlled with a PID controller where the Plant is the winch together with the pendulum. The Manipulated Variable is the winch speed, the pendulum weight z-position is the Process Variable, and the Set Point is the desired height of the load above the xy-plane at z equal to zero. The winch base has an oscillating vertical motion which moves the pendulum up and down while the PID controller is counteracting this vertical motion plus the vertical motion due to the swing of the pendulum load. Note that the tutorial code doesn’t contain any graphical nodes.

../_images/pid_controller_scene.png

Fig. 29.2 This is the pendulum scene in the tutorial. The green cube represents the winch with an oscillating vertical motion. The red sphere is the pendulum load that the PID controller is stabilizing and the gray transparent box is the vertical Set Point for the PID controller.

The first part of our tutorial is an implementation of the agxModel::ControllerHandler::Plant base class that can read process data and set signal values on the system we are controlling. In this specific case the Plant Process Variable is the z-position of the pendulum weight and the Manipulated Variable is the winch speed controlling the length of the wire. The class overrides two abstract methods in agxModel::ControllerHandler::Plant, the methods getProcessVariable and setManipulatedVariable, see code below. Both the getProcessVariable and the setManipulatedVariable are called in the post step of the agxModel::ControllerHandler.

/// A thin implementation of the Plant base class. This implementation reads the
/// pendulum position and sets the winch speed.
class PlantWinch : public agxModel::ControllerHandler::Plant
{
public:
  PlantWinch(agxWire::Winch* winch, agx::RigidBody* pendulumWeight) :
      m_winch(winch), m_pendulumWeight(pendulumWeight)
  {
  }

  /// \return The current measured Process Variable (PV).
  agx::Real getProcessVariable(const agx::TimeStamp& time) override
  {
    // Update the pendulum base oscillating motion.
    // Note: It is not considered best practice to modify the simulation in this method.
    agx::Real winchBaseVelocity = 0.5 * std::cos(1 / 10.0 * time);
    m_winch->getRigidBody()->setVelocity(agx::Vec3(0, 0, winchBaseVelocity));

    // Here the Plant Value is the z position of the load attached to the wire in the winch.
    return m_pendulumWeight->getPosition().z();
  }

  /// Set the Manipulated Variable (MV) controlling the Plant, also known as Control Variable.
  void setManipulatedVariable(agx::Real manipulatedVariable) override
  {
    // The PID manipulate variable is set as the winch speed.
    // Negative since a negative error should pull in the weight.
    m_winch->setSpeed(-manipulatedVariable);
  }

private:
  agxWire::WinchRef m_winch;
  agx::RigidBodyRef m_pendulumWeight;
};

The next part of our example is to setup the pendulum and add a Control System with a PID controller to the simulation. In our control system, agxModel::ControllerHandler, the above specified PlantWinch is added together with an agxModel::PidController1D.

agxSDK::SimulationRef sim = new agxSDK::Simulation;

RigidBodyRef pendulumWeight = new RigidBody(new agxCollide::Geometry(new agxCollide::Sphere(1)));
pendulumWeight->setPosition(-0.5, 0.5, -10);

// Create wire to hang between the winch base and the winch load
agxWire::WireRef wire = new agxWire::Wire(0.015, 1, false);
// Attach wire to load
wire->add(new agxWire::BodyFixedNode(pendulumWeight));
// Wire nodes
wire->add(new agxWire::FreeNode(agx::Vec3(0, 0, 0)));

agx::RigidBodyRef winchBase = new agx::RigidBody("winchBase");
winchBase->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(1, 1, 1))));
winchBase->setMotionControl(agx::RigidBody::KINEMATICS);

agxWire::WinchRef winch = new agxWire::Winch(winchBase, agx::Vec3(0, 0, 0), agx::Vec3(0, 0, 1));
agx::Real pulledInLength = 10;
// attach wire to winch
wire->add(winch, pulledInLength);

// Create the Plant describing the process we should control. The class, PlantWinch,
// implements the base class agxModel::ControllerHandler::Plant which reads the
// pendulum z-position and sets the winch speed.
agxModel::ControllerHandler::PlantRef plant = new PlantWinch(winch, pendulumWeight);

agx::Real setPoint = -10;
agx::Real proportionalGain = 0.1;
agx::Real integralGain = 0.05;
agx::Real derivativeGain = 5;
agxModel::PidController1DRef pidController = new agxModel::PidController1D();
pidController->setGains(proportionalGain, integralGain, derivativeGain);
pidController->setSetPoint(setPoint);

agxModel::ControllerHandlerRef controller_handler =
  new agxModel::ControllerHandler(pidController, plant);
sim->add( pendulumWeight );
sim->add( winchBase );
sim->add( wire );
sim->add( controller_handler );

29.5. Robot control example

This section will describe how to control the position of a robot by driving each joint with a motor that adds forces to the joint, to keep it in the right position. Each robot joint will be controlled independently, based on a trajectory file describing the angle of each joint at each time step. A drive line together with a PID controller will then be used to add the correct torques to the joints.

Most of the scripts used in this demo are located under the folder RobotControl among the python tutorials.

29.5.1. PID controller demo

We start with a simple example of how to use a PID controller on a single joint arm. First we take a look at the PID controller. We use a simple example, which can be found in Figure 11.15 in Modern Robotics: Mechanics, Planning, and Control, by K. M. Lynch and F. C. Park.

29.5.1.1. PID controller

The PID controller is implemented as a StepEventListener and will need the joints we want to control, the file with the trajectory and the PID tuning parameters. The heart of the PID controller is the calculation of the regulator torque, \(\tau\), which mathematically can be expressed as

\[\tau = K_p \theta_e + K_d \dot{\theta_e} + K_i \int \theta_e(t) dt,\]

where \(\theta_e\) is the difference between the current angle and the reference angle and \(K_p\), \(K_d\) and \(K_i\) are the tuning parameters.

In the controller used in this demo, \(\dot{\theta_e}\) is calculated as the difference between the wanted angular velocity and the current velocity, where the velocities are approximated by dividing the change in angle from the previous step with the time step. The integral part is approximated by adding up the error, \(\theta_e\), multiplied with the time step.

For more details about the implementation of the PID controller, see pid_controller.py.

In pid_controller.py there is also a class called RobotJoint, which is used to save a hinge and the lookup table used to set the torque for that hinge. The hinges we want to control must be saved into a RobotJoint before it is sent into the PID controller.

29.5.1.2. Single joint simulation

To test the PID controller a single joint arm is created. It is simply a long and thin box connected to a static body with a hinge. To drive the hinge a drive line with an engine is created, connected to the hinge with a gear. The engine is mainly used since it makes it simple to control what torque is applied to the drive line. The torque is added as the only value in the engine lookup table, which is saved together with the hinge as a RobotJoint.

hinge.getMotor1D().setEnable(False)

powerLine = agxPowerLine.PowerLine()
sim.add(powerLine)

engine = agxDriveTrain.Engine()
engine.setPowerGenerator(agxPowerLine.TorqueGenerator(engine.getRotationalDimension()))
engine.setThrottle(1)
lookupTable = engine.getPowerGenerator().getPowerTimeIntegralLookupTable()
powerLine.add(engine)

actuator = agxPowerLine.RotationalActuator(hinge)
gear = agxDriveTrain.HolonomicGear()
engine.connect(gear)
gear.connect(actuator)

joint = RobotJoint(hinge, lookupTable)

The PID controller must know what angle the joint should have at what time, and in this case it reads it from a trajectory file. This file must thus first be created. As this example only has one joint, it is easy to add the angles and corresponding times into each line of a file. We let the arm move up to \(\frac{\pi}{2}\) rad in 2 seconds, rest there for a second and then move down again in 2 seconds. To get a smooth trajectory the controller gets one angle position for each time step, except when the arm should be standing still.

time = np.append(np.linspace(0, 2.0, 2000), np.linspace(3.0, 5.0, 2000))
angle = np.append(np.linspace(0, agx.PI/2, 2000), np.linspace(agx.PI/2, 0, 2000))

fileName = "single_joint_trajectory.txt"
file = open(fileName, "w")
for i in range(0, len(time)):
    file.write("{0:.4f} {1:.6f}\n".format(time[i], angle[i]))
file.close()

Having created the trajectory file it is possible to create the PID controller and add it to the simulation.

Kp = 300.0
Kd = 50.0
Ki = 50.0
controller = PidController([joint], Kp, Kd, Ki, fileName)
sim.add(controller)

The PID tuning parameters of the controller are only a suggestion, but running the example for the single joint arm with those values, should give the results seen in the figure below.

../_images/robot_control_1.png

Fig. 29.3 Hinge angle for each time step. Dashed line is the reference.

For more details see tutorial_pid_controller.agxPy.

29.5.2. Generic robot demo

In this tutorial it is demonstrated how to control a robot with six degrees of freedom. The robot used is the GenericRobot. See figure below.

../_images/robot_control_2.png

Fig. 29.4 The robot called generic robot.

29.5.2.1. Generating a trajectory

To control the robot and move it into some wanted position or pattern it must first be known how each of the joints in the robot should move. In this case the joints are controlled independently and a file is created with angle positions for each joint at each time step.

Normally the angles of the joints are not known, but what is known is the position of where the end effector should be. This is a problem that can be solved in more than one way, but here only one, simple, solution is demonstrated. The approach is to drag the end effector into the wanted position, and collect the joint angles during the motion. This is done by creating a kinematic body and connecting it to the robot head. The kinematic body is then moved in straight lines to the wanted positions. One must be careful not to move this body into a position outside the reach of the robot. The velocity of this body is constant between the positions, but for better results it is possible to make it slow down as it comes closer to its destination.

In the example in tutorial_robot_trajectory.agxPy the kinematic body moves in a square in a horizontal plane and saves that angle data into a file named trajectory_square.txt.

29.5.2.2. Controlling a robot

Having the trajectory of the robot, it is possible to try to move the robot in the same pattern using a PID controller and something that drives the joints. Just as for the single joint example an engine in a drive train is connected to the hinge with a gear and in this case we set the engine inertia and gear ratio to a given value. Then a RobotJoint must be created for each joint using the correct hinges and lookup tables.

# Save all the hinges of the robot in a list
hinges = [robot.hinges["bottom"], robot.hinges["bottomMiddle"], robot.hinges["middleTop"],
          robot.hinges["topHead"], robot.hinges["head"], robot.hinges["headPlate"]]

# List of values for motor inertia and gear ratio.
motorInertia = [0.27, 0.046, 0.036, 0.15, 0.15, 0.18]
gearRatio = [125.0, 171.0, 143.0, 60.0, 67.0, 50.0]

powerLine = agxPowerLine.PowerLine()
sim.add(powerLine)

joints = []
for i in range(0, len(hinges)):
    hinges[i].getMotor1D().setEnable(False)
    engine = agxDriveTrain.Engine()
    engine.setPowerGenerator(agxPowerLine.TorqueGenerator(engine.getRotationalDimension()))
    lookupTable = engine.getPowerGenerator().getPowerTimeIntegralLookupTable()
    engine.setThrottle(1.0)
    engine.setInertia(motorInertia[i])
    powerLine.add(engine)

    actuator = agxPowerLine.RotationalActuator(hinges[i])
    gear = agxDriveTrain.HolonomicGear()
    gear.setGearRatio(gearRatio[i])

    engine.connect(gear)
    gear.connect(actuator)

    joints.append(RobotJoint(hinges[i], lookupTable))

A PID controller can then be created. Note that only one controller is needed. The joints should all be put in a list, that is sent into the controller. The tuning parameters can either be set to the same value for all joints, by just sending in one value, or to different values for each joint. If one wants different parameter values for each joint, the parameters are put in a list instead. The order has to be the same as for the joints, i.e. the first value is for the first joint and so on.

sim.add(PidController(joints, 900, 200, 600, "trajectory_square.txt"))

Running the tutorial with the square example with the above controller should give the result seen in the figure below.

../_images/robot_control_3.png

Fig. 29.5 Robot tool position in the x-y-plane. The values are in meters and the dashed line is the reference. The robot follows the straight line to the square, then moves clockwise around the square path.

For more details see tutorial_robot_control.agxPy.

29.6. 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 or pfrl.

../_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.

29.6.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, info->dict). This method is called in env.step().

  • _setup_gym_environment_spaces - Creates and sets the action, observation and reward range attributes

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()
# This initializes graphics
env.init_render(mode="rgb_array", sync_real_time=False)
env.reset()
for _ in range(10):
  env.step(env.action_space.sample())
  # This renders and displays a new frame
  imgs = env.render(mode="rgb_array")

The mode can be "human" or rgb_array. With mode="human" a graphics window is created for viewing the simulation, for headless rendering use mode="rgb_array". Calling images = env.render(mode="rgb_array") will return a list of images 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.

29.6.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.

29.6.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.

29.6.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.

29.6.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.

29.6.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.