.. include:: definitions.rstinc
Robotics
==========
This chapter is a collection of topics relevant for robotics and control.
.. contents::
:depth: 1
:local:
.. _urdf_reader:
URDF
-----
AGX Dynamics provides a URDF reader that takes a URDF file and returns an AGX Dynamics
representation of the model.
UrdfReader class
^^^^^^^^^^^^^^^^^^
The :cpp:`agxModel::UrdfReader` class has a static :cpp:`read` method for reading urdf files and returning an :cpp:`agxSDK::Assembly` object with all the robot parts (bodies, geometries and constraints).
The behavior of the read method can be controlled using the :cpp:`agxModel::UrdfReader::Settings` class. See the examples below for more information.
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.
C++ example
~~~~~~~~~~~~~~
Read a URDF model and access one of its joints.
.. code-block:: c++
// 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 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);
Python example
~~~~~~~~~~~~~~~~
Read a URDF model and access one of its joints.
.. code-block:: python
# 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 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)
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:
.. code-block:: console
cd Algoryx/AGX-version/data/python/ROS2
3. Run the URDF Panda ROS2 controller script by calling python, i.e:
.. code-block:: console
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.
.. code-block:: text
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 ☓
AGX Dynamics representation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Below is a detailed description of the how the URDF features are represented by AGX Dynamics.
~~~~~~~~~~
The root element **robot** is represented by an ``agxSDK::Assembly``. The name of the ``agxSDK::Assembly``
is set to the required *name* attribute.
~~~~~~~~~~
Each **link** is represented by an ``agx::RigidBody``. The name of the ``agx::RigidBody`` is set to
the required *name* attribute.
@@@@@@@@@@
The optional **inertial** element determines the mass and intertial properties of the ``agx::RigidBody``. If no
**inertial** element exist for a **link**, then AGX Dynamis will automatically calculate the mass properties based on
all **visual** elements. The reason for using the visual elements is that it in most cases will have a more correct volumetric representation of the robot compared to the **collide** elements.
The behaviour of the reader can be controlled using the :cpp:`UrdfReader::Settings` class.
++++++++
The optional **origin** element determines the center of mass of the ``agx::RigidBody``. If not specified
(but **inertial** is), then identity position and orientation is used.
++++++
The **mass** element determines the mass of the ``agx::RigidBody``. This is a required attribute for any
**inertial** element.
+++++++++
The **inertia** element determines the inertia of the ``agx::RigidBody`` with respect to its center of mass.
This is a required attribute for any **inertial** element.
@@@@@@@@
Each **visual** element is represented by a ``agxCollide::Geometry`` with collision disabled. The name of
the ``agxCollide::Geometry`` is set to the optional *name* attribute if it is specified.
++++++++
The optional **origin** element determines the position and orientation of the ``agxCollide::Geometry`` with respect to the owning
``agx::RigidBody``. Identity position and orientation is used if not specified.
++++++++++
The required **geometry** element is represented by an ``agxCollide::Shape`` where **box**,
**cylinder** and **sphere** are represented by the corresponding AGX Dynamics primitive shapes. Any **mesh**
element is represented by a ``agxCollide::Trimesh``.
++++++++++
The optional **material** element is represented by a ``agxCollide::RenderMaterial``. Note that if a **texture** is
specified, the texture information will be lost if a serialize/deserialize cycle of the scene is done and
the underlying texture resouce (pointed to by the *filename* attribute) is moved or removed.
@@@@@@@@@@@
Each **collision** element is handled similarly as a **visual** element but has collision enabled.
Any ``agxCollide::Geometry`` representing a **collision** element is given empty ``agxCollide::RenderData``
meaning it will not be rendered to the screen if not debug rendering is active. Since it is not
rendered, any **material** element inside a **collision** element is ignored. Note that a **collision**
element does not affect the automatically calculated mass properties of a ``agx::RigidBody`` representing
the parent **link** in the case where no **inertial** element exists. Only the **visual** elements are
used in the calculation for that case.
@@@@@@@@@@@
Each **joint** element is represented by an ``agx::Constraint``. The name of the ``agx::Constraint``
is set to the required *name* attribute. Any *revolute* **joint** is represented by an ``agx::Hinge`` with the
secondary constraint ``agx::RangeController`` active. Any *continuous* **joint** is represented by an
``agx::Hinge``. Any *prismatic* **joint** is represented by an ``agx::Prismatic`` with the secondary
constraint ``agx::RangeController`` active. Any *fixed* **joint** is represented by an ``agx::LockJoint``.
Any *floating* **joint** is ignored since free body movement is the default behaviour of any ``agx::RigidBody``.
Any *planar* **joint** is represented by an ``agx::PlaneJoint``.
++++++++
The optional **origin** element specifies the position and orientation of the child ``agx::RigidBody``
representing the child **link** with respect to the parent ``agx::RigidBody`` representing the parent
**link**. If not specified identity position and orientation is used.
++++++++
The required *link* attribute of the required **parent** element specifies the name of the parent
``agx::RigidBody`` that is attached to the ``agx::Constraint`` representing the **joint**.
++++++++
The required *link* attribute of the required **child** element specifies the name of the child
``agx::RigidBody`` that is attached to the ``agx::Constraint`` representing the **joint**.
++++++++
The optional **axis** element determines the axis of rotation for any ``agx::Hinge`` representing
a *revolute* or *continuous* **joint**, or the axis of translation for any ``agx::Prismatic``
representing a *prismatic* **joint**. If not specified, then [1, 0, 0] is used.
++++++++++++++++
Not supported and will be ignored.
++++++++++++++++
The optional **dynamics** element determines the dynamic behaviour of the ``agx::Constraint`` representing a
**joint**. If not specified the default AGX Dynamics parameters are used for the ``agx::Constraint``.
damping
########
Not supported and will be ignored. Note that it is possible to manually set the damping for any DOF for a
``agx::Constraint`` representing a **joint**. This can be done by accessing the ``agx::Constraint``
from the returned ``agxSDK::Assembly`` followed by calling the function `agx::Constraint::setDamping(...)`.
friction
#########
The optional *friction* attribute specifies the static friction of the joint in Newtons. This
corresponds to a force range of the secondary constraint ``agx::FrictionController`` of the
``agx::Constraint`` and is applied as such.
If the *friction* attribute is not specified, the default AGX Dynamics friction is used.
++++++++
The **limit** element specifies the upper and lower bounds of of the **joint** both in terms of
rotational/translational freedom and in maximal force/torque.
The **limit** element is supported only for *revolute* and *prismatic* **joint**'s and for those
it is also a required element.
The *lower* and *upper* attributes are represented as the lower and upper range of the secondary
constraint ``agx::RangeController`` and are applied as such. If not specified, then 0 is used for
both attributes.
The required *effort* attribute specifies the maximum force/torque of the ``agx::Constraint``'s secondary
constraints: ``agx::RangeController``, ``agx::LockController`` and ``agx::TargetSpeedController``.
The *velocity* attribute is not supported and will be ignored.
++++++++
The optional **mimic** element makes it possible to configure an ``agx::Constraint`` representing
the **joint** in such a way that it will move in accordance with another ``agx::Constraint``.
Internally any **mimic** element is represented as a ``agxPowerLine::PowerLine`` with a number of
``agxPowerLine::Connector``'s together with an ``agxDriveTrain::Gear`` configured to give this
behaviour. The **mimic** feature is supported for *revolute*, *continuous* and *prismatic* **joint**'s.
Having a *revolute* or *continuous* **joint** mimic a *prismatic* **joint** or vice versa is not supported,
but having a *revolute* **joint** mimic a *continuous* **joint** or vice versa is.
multiplier
###########
The optional *multiplier* attribute corresponds to a gear ratio of the ``agxDriveTrain::Gear`` that
is part of the ``agxPowerLine::PowerLine`` representing the **mimic** element.
If not specified the value 1 is used.
offset
###########
The optional *offset* attribute will add an initial rotational or translational offset to the
``agx::Constraint`` representing the **joint** with the **mimic** element. If an initial joint
value is set using the `joints` parameter when calling the ``agxModel::UrdfReader::read()`` function,
the *offset* attribute will be added to this initial joint value.
++++++++++++++++++++
Not supported and will be ignored.
~~~~~~~~~~~~~~~~
Not supported and will be ignored.
~~~~~~~~~~~~~~
Not supported and will be ignored.
~~~~~~~~~~~~~~
Not supported and will be ignored.
~~~~~~~~~~~~~~~~
Not supported and will be ignored.
.. include:: agxros2.rstinc
.. _inverse_dynamics:
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.
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
.. code-block:: c++
const agxSDK::Assembly* internalMotionData = id->getMotionData();
to get the final state of the system used for calculating the trajectories. Or
.. code-block:: c++
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.
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.
.. image:: 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.
.. image:: 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.
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.
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.
.. _agxmodel__pid_controller:
PID Controller
---------------------------
In :cpp:`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:
.. math::
u(t) = MV(t) = K_p e(t) + K_i \int_{0}^{t} e(\tau) d\tau + K_d \frac{de}{dt},
where :math:`u` is the out signal, also known as the Manipulated Variable (MV), and :math:`K_p`, :math:`K_i`, and :math:`K_d` are the tuning parameters or proportional gain, integral gain, and derivative gain. In the figure below, :numref:`fig-pid_diagram`, 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 :math:`u(t)`. The error, :math:`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 :math:`\frac{de}{dt} = \frac{e - e_{n-1}}{t - t_{n-1}}`, where :math:`e_{n-1}` is the error from the previous time step and :math:`t - t_{n-1}` is the time step. The integral part is approximated by adding up the error, :math:`e`, multiplied by the time step. This explicit solution of :math:`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.
.. _fig-pid_diagram:
.. figure:: images/pid_diagram.png
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). :math:`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.
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 :numref:`fig-pid_controller_scene`. 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.
.. _fig-pid_controller_scene:
.. figure:: images/pid_controller_scene.png
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`.
.. code-block:: c++
/// 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`.
.. code-block:: c++
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 );
.. include:: robot_control.rstinc
.. include:: reinforcement_learning.rstinc