.. _urdf_reader: ==== URDF ==== AGX Dynamics provides a URDF reader that takes a URDF file and returns an AGX Dynamics representation of the model. -------- 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. ^^^^^^^^^^^ Example C++ ^^^^^^^^^^^ 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}; // (Optional) Determines if the base link of the URDF model should be attached to the // world or not. bool attachToWorld = true; // (Optional) If true (default) the collision between linked/constrained bodies will be disabled. bool disableLinkedBodies = true; // Read the URDF. agxSDK::AssemblyRef model = agxModel::UrdfReader::read(urdfFile, urdfPackagePath, &initJointPos, attachToWorld, disableLinkedBodies); // 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); ^^^^^^^^^^^^^^ Example Python ^^^^^^^^^^^^^^ 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) # (Optional) Determines if the base link of the URDF model should be attached to the # world or not. attachToWorld = True # (Optional) If true (default) the collision between linked/constrained bodies will be disabled. disableLinkedBodies = True # Read the URDF. model = agxModel.UrdfReader.read(urdfFile, urdfPackagePath, initJointPos, attachToWorld) # 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) ^^^^^^^^^^^^^^^^^^^^^^^ Example URDF Panda ROS2 ^^^^^^^^^^^^^^^^^^^^^^^ To run this example, ROS2 must be installed. See section :numref:`ros2-and-agx-python` for instructions of how to install and set up ROS2. **Start the URDF Panda robot simulation** 1. Open a new "AGX Dynamics Command Line". 2. Call the ROS2 local_setup.bat to setup the ROS2 environment variables. Depending on if you are using the embedded python that comes with AGX Dynamics, or the system installed one, the path to the ros2 library might be different. By default the local_setup.bat is located in your ros2-algoryx installation directory, eg: .. code-block:: console Algoryx/AGX-version/python-x64/site-packages/ros2-algoryx/ros2/local_setup.bat 3. In the AGX command prompt, cd to the AGX ROS2 examples directory, e.g: .. code-block:: console cd Algoryx/AGX-version/data/python/ROS2 4. Run the URDF Panda ROS2 script by calling python, i.e: .. code-block:: console python urdf_panda_ros2.py **Start the URDF Panda ROS2 controller** 1. Follow steps 1-3 from above. 2. 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 if the ``agx::RigidBody``. If no **inertial** element exist for a **link**, AGX Dynamic's automatically calculated mass properties given all **visual** elements are used for the ``agx::RigidBody`` instead. ++++++++ ++++++++ 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.