Articulated Systems¶

ANYmal robots (B and C versions) simulated in RaiSim.
Introduction¶
An articulated Systems is a system of multiple bodies connected through joints. There are two types of articulated systems: Kinematic trees and closed-loop systems. Kinematic trees do not contain a loop (a body always has only one parent joint). Consequently, the number of joints is equal to the number of bodies (for floating systems, we assign a floating joint on the root body). RaiSim can only simulate kinematic trees.
Since all bodies have one joint, a body index is always the same as its parent joint’s index. A body refers to a rigid body which is composed of one or more links. All links within a body are connected to each other by a fixed joint.
Creating an instance¶
Just like any other object, an articulated system is created by the world instance using addArticulatedSystem
method.
There are three ways to specify the system.
by providing the path to the URDF file (the most recommneded way)
by providing
std::string
of the URDF text (can be useful when working with Xacro)by providing a
raisim::Child
instance. It is an advanced method and not recommnded to beginners.
Note that option 1 and 2 use the same method. You can provide either the path string or the contents string and the class will identify which one is provided.
State Representation¶
The state of an articulated system can be represented by a generalized state \(\boldsymbol{S}\), which is composed of a generalized coordinate \(\boldsymbol{q}\) and a generalized velocity \(\boldsymbol{u}\). Since we are not constraining their parameterization, in general,
A generalized coordinate fully represents the configuration of the articulated system and a generalized velocity fully represents the velocity state of the articulated system.
Every joint has a corresponding generalized coordinate and generalized velocity.
A concatenation of all joint generalized coordinates and velocities are the generalized coordinates and velocities of the articulated system, respectively.
The order of this concatenation is called joint order.
The joint order is determined by the order the joint appears in the URDF unless otherwise it is explicitly given at the instance construction time.
The joint order can be accessed through getMovableJointNames()
.
Note the keyword “movable”.
The fixed joints contribute to neither the generalized coordinate nor the generalized velocity.
Only movable joints do (with one exception metioned below).
The joint order starts with the root body which is the first body of the articulated system.
For floating-base systems, the root body is the floating base.
For fixed-base systems, the root body is the one rigidly attached to the wolrd.
Even though the fixed base cannot move physically, users can move them using setBaseOrientation
and setBasePosition
.
So getMovableJointNames()
method will return the fixed base name and the fixed base joint is a part of the joint order.
The following example illustrates how the generalized coordinates and velocities are defined.
To set the state of the system, the following methods can be used
setGeneralizedCoordinate
setGeneralizedVelocity
setState
To obtain the state of the system, the following methods can be used
getGeneralizedCoordinate
getGeneralizedVelocity
getState
The dimensions of each vector can be obtained respectively by
getGeneralizedCoordinateDim
getDOF
orgetGeneralizedVelocityDim
.
These two methods are identical
Joints¶
Here are the available joints in RaiSim.
Fixed |
Floating |
Revolute |
Prismatic |
Spherical |
|
---|---|---|---|---|---|
\(|\boldsymbol{u}|\) |
0 |
6 |
1 |
1 |
3 |
\(|\boldsymbol{q}|\) |
0 |
7 |
1 |
1 |
4 |
Velocity |
\(m/s\), \(rad/s\) |
\(rad/s\) |
\(m/s\) |
\(rad/s\) |
|
Position |
\(m\), \(rad\) |
\(rad\) |
\(m\) |
\(rad\) |
|
Force |
\(N\), \(Nm\) |
\(Nm\) |
\(N\) |
\(Nm\) |
The generalized coordinates/velocities of a joint are expressed in the joint frame and with respect to the parent body. Joint frame is the frame attached to every joint and fixed to the parent body. Parent body is the one closer to the root body among the two bodies connected via the joint. Note that the angular velocity of a floating base is also expressed in the parent frame (which is the world frame). Other libraries (e.g., RBLD) might have a different convention and a special care has to be taken in conversions.
URDF convention¶
RaiSim uses a modified URDF protocol to define an articulated system. URDF files following the original convention can be read in RaiSim. However, since RaiSim offers more functionalities, a RaiSim URDF might not be read by other libraries following the original URDF convention.
The modifications are as follows:
Capsule geom is available for both collision objects and visual objects (with the keyword “capsule”). The geom is defined by “height” and “radius” keyword. The height represents the distance between the two center points of the spheres.
A <joint>/<dynamics> tag can have three more attributes: rotor_inertia, spring_mount and stiffness.
Here is an example joint with the raisim tags
<joint name="link1Tolink2" type="spherical">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 -0.24"/>
<axis xyz="0 1 0"/>
<dynamics rotor_inertia="0.0001" spring_mount="0.70710678118 0 0.70710678118 0" stiffness="500.0" damping="3."/>
</joint>
Rotor_inertia in Raisim approximately simulates the rotor inertia of the motor (but missing the resulting gyroscopic effect, which is often neglegible).
It is added to the diagonal elements of the mass matrix.
It is common way to include the inertial effect of the rotor.
You can also override it in C++ using setRotorInertia()
.
In RaiSim, each body of an articulated system has a set of collision bodies and visual objects.
Collision bodies contain a collision object of one of the following shapes: mesh, sphere, box, cylinder, capsule.
Visual objects just store specifications for visualization and the actual visualzation happens in a visualizer (e.g., raisimOgre)
For details, check the URDF protocol.
Kinematics¶
Frames¶
A position and velocity of a specific point on a body of an articulated system can be obtained by attaching a frame. Frames are rigidly attached to a body of the system and has a constant position and orientation (w.r.t. parent frame). It is the most recommended way to get kinematics information of a point of an articulated system in RaiSim.
All joints have a frame attached and their names are the same as the joint name. To create a custom frame, a fixed frame can be defined at the point of the interest. A dummy link with zero inertia and zero mass must be added on one side of the fixed joint to complete the kinematic tree.
A frame can be locally stored as an index in the user code. For e.g.,
#include “raisim/World.hpp”
int main() {
raisim::World world;
auto anymal = world.addArticulatedSystem(PATH_TO_URDF);
auto footFrameIndex = world.getFrameIdxByName("foot_joint"); // the URDF has a joint named "foot_joint"
raisim::Vec<3> footPosition, footVelocity, footAngularVelocity;
raisim::Mat<3,3> footOrientation;
anymal.getFramePosition(footFrameIndex, footPosition);
anymal.getFrameOrientation(footFrameIndex, footOrientation);
anymal.getFrameVelocity(footFrameIndex, footVelocity);
anymal.getFrameAngularVelocity(footFrameIndex, footAngularVelocity);
}
You can also store a Frame reference.
For e.g., you can replace getFrameIdxByName
by getFrameByName
in the above example.
In this way, you can access the internal variables and even modify them.
Modifying the frames do not affect the joints.
Frames are instantiated during initialization of the articulated system instance and affect neither kinematics nor dynamics of the system.
Jacobians¶
Jacobians of a point in RaiSim satisfy the following equation:
where \(\boldsymbol{v}\) represents the linear velocity of the associated point. If a rotational Jacobian is used, the right-hand side changes to a rotational velocity expressed in the world frame.
To get the Jacobians associated with the linear velocity, the following methods are used
getSparseJacobian
getDenseJacobian
– this method only fills non-zero values. The matrix should be initialized to a zero matrix of an appropriate size.
To get the rotational Jacobians, the following methods are used
getSparseRotationalJacobian
getDenseRotationalJacobian
– this method only fills non-zero values. The matrix should be initialized to a zero matrix of an appropriate size.
The main Jacobian class in RaiSim is raisim::SparseJacobian
.
RaiSim uses only sparse Jacobians as it is more memory-friendly.
Note that only the joints between the child body and the root body affect the motion of the point.
The class raisim::SparseJacobian
has a member idx
which stores the indicies of columns whose values are non-zero.
The member v
stores the Jacobian except the zero columns.
In other words, ith column of v
corresponds to idx[i]
generalized velocity dimension.
Dynamics¶
All force and torque acting on the system can be represented as a single vector in the generalized velocity space. This representation is called generalized force \(\boldsymbol{\tau}\). Just like in a Cartesian coordiate (i.e., x, y, z axes), the power exerted by an articulated system is computed as a dot product of generalized force and generalized velocity (i.e., \(\boldsymbol{u}\cdot\boldsymbol{\tau}\)).
We can also combine the mass and inertia of the whole articulated system and represent them in a single matrix. This matrix is called mass matrix or inertia matrix and denoted by \(\boldsymbol{M}\). A mass matrix represents how much the articulated system resists change in generalized velocities. Naively speaking, a large mass matrix means that the articulated system experiences a low velocity change for a given generalized force.
The total kinetic energy of the system is computed as \(\frac{1}{2}\boldsymbol{u}^T\boldsymbol{M}\boldsymbol{u}\).
This quantity can be obtained by getKineticEnergy()
.
The total potential energy due to the gravity is a sum of \(mgh\) for all bodies.
This quantity can be obtained by getEnergy()
.
Note that the gravity has to be specified since only the world has the gravity vector.
The equation of motion of an articulated system is shown below:
Here \(\boldsymbol{h}\) is called a non-linear term. There are three sources of force that contributes to the non-linear term: gravity, coriolis, and centrifugal force. It is rarely useful to compute the gravity contribution to the nonlinear term alone. However, if it is needed, the easiest way is to make the same robot in another world with zero velocity. If the generalized velocity is zero, the coriolis and centrifugal contributions are zero.
The following methods are used to obtain dynamic quantities
getMassMatrix()
getNonlinearities()
getInverseMassMatrix()
PD Controller¶
When naively implemented, a PD controller can often make a robot unstable. However, this is often not so bad for robotics since this instability is also present in the real system (discrete-time control system).
For other applications like animation and graphics, it is often desirable to have a stable PD controller when a user wants to keep the time step small. Therefore, this PD controller exploits a more stable integration scheme and can have much smaller time step than a naive implementation.
To use this PD controller, you have to set the desirable control gains first
Eigen::VectorXd pGain(robot->getDOF()), dGain(robot->getDOF());
pGain<< ...; // set your proportional gain values here
dGain<< ...; // set your differential gain values here
robot->setPdGains(pGain, dGain);
Note that the dimension of the pGain vector is the same as that of the generalized velocity NOT that of the coordinate.
Finally, the target position and the velocity can set as below
Eigen::VectorXd pTarget(robot->getGeneralizedCoordinateDim()), vTarget(robot->getDOF());
pTarget<< ...; // set your position target
vTarget<< ...; // set your velocity target
robot->setPdTarget(pTarget, vTarget);
Here, the dimension of the pTarget vector is the same as that of the generalized coordinate NOT that of the velocity. This is confusing and might seem inconsistent. However, this is a valid convention. The only reason that the two dimensions differ is quaternions. The quaternion target is represented by a quaternion whereas the virtual spring stiffness between the two orientations can be represented by a 3D vector, which is composed of motions in each angular velocity components.
A feedforward force term can be added by setGeneralizedForce()
if desired.
This term is set to zero by default.
Note that this value is stored in the class instance and does not change unless the user specifies it so.
If this feedforward force should be applied for a single time step, it should be set to zero in the subsequent control loop (after integrate()
call of the world).
The theory of the implemented PD controller can be found in chapter 1.2 of this article. This document is only for advanced users and it is not necessary to use RaiSim.
Integration Steps¶
Integration of an articulated systems is performed in two stages: integrate1
and integrate2
The following steps are performed in integrate1
If the time step is changed, update the damping of the mass matrix (which reflects effective inertial increase due to springs, dampers and PD gains)
Update positions of the collision bodies
Detect collisions (called by the world instance)
The world assigns contacts on each object and computes the contact normal
Compute the mass matrix, nonlinear term and inverse inertia matrix
Compute (Sparse) Jacobians of contacts
After this step, all kinematic/dynamic proerpties are computed at this stage.
Users can access them if they are necessary for the controller.
Next, integrate2
computes the rest of the simulation.
Compute contact Properties
Compute PD controller (if used), add it to the feedforward force and bound it by the limits
Compute generalized forces due to springs and external forces/torques
Contact solver (called by the world instance)
Integrate the velocity
Integrate the position (in a semi-implicit way)
Get and Modify Robot Description in code¶
RaiSim allows users modify most of the robot parameters freely in code. This allows users to create randomized robot model, which might be useful for AI applications (i.e., dynamic randomization). Note that a random model might be kinematically and dynamically unrealistic. For example, joints can be locked by collision bodies. In such cases, simulation cannot be performed reliably and it is advised to carefully check randomly generated robot models.
Here is a list of modifiable kinematic/dynamic parameters.
Joint Position (relative to the parent joint) Expressed in the Parent Frame
getJointPos_P
method returns (a non-const reference to) a std::vector
of position vectors from the parent joint to the child joint expressed in the respective parent joint frames.
This should be changed with care since it can result in unrealistic collision geometry.
This method does not change the position of the end-effector with respect to its parent as the position of the last link is defined by the collision body position, not by the joint position.
The elements are ordered by the joint indicies.
Joint Axis in the Parent Frame
getJointAxis_P
method returns (a non-const reference to) a std::vector
of joint axes expressed in the respective parent joint frame.
This method should also be changed with care.
The elements are ordered by the joint indicies.
Mass of the Links
getMass
method returns (a non-const reference to) a std::vector
of link masses.
IMPORTANT! You must call :code:`updateMassInfo` after changing mass values.
The elements are ordered by the body indicies (which is the same as the joint indicies in RaiSim).
Center of Mass Position
getBodyCOM_B
method returns (a non-const reference to) a std::vector
of the COM of the bodies.
The elements are ordered by the body indices.
Link Inertia
getInertia
method returns (a non-const reference to) a std::vector
of link inertia.
The elements are ordered by the body indicies.
Collision Bodies
getCollisionBodies
method returns (a non-const reference to) a std::vector
of the collision bodies.
This vector contains all collision bodies associated with the articulated system.
getCollisionBody
method returns a specific collision body instead.
All collision bodies are named “LINK_NAME” + “/INDEX”.
For example, the 2nd collision body of a link named “FOOT” is named “FOOT/1” (1 because the index starts from 0).
The collision bodies is a class that contains position/orientation offset from the parent joint frame, name, parent body index, and ODE collision pointer (dGeomID
, retrieved using getCollisionObject
).
The collision geom can be modified using ODE methods (ODE manual).
Users can also modify the material of the collision body.
This material affects the contact dynamics.
Apply External Forces/torques¶
The following two methods are used to apply external force and torque respectively
setExternalForce
setExternalTorque
Collision¶
Apart from the collision mask and collision group set in the world, users can also disable a collision between a certain pair of the links with ignoreCollisionBetween
.
Types of Indicies¶
ArticulatedSystem class contains multiple types of indicies. To query a specific quantity, you have to provide an index of the right type. Here are the types of indicies in Articulated Systems
Body/Joint Index: All fixed bodies are combined to a single movable body. Each movable body has a unique body index. Because there is a movable joint associated with a movable body, there is a 1-to-1 mapping between the joints and the bodies and they share the same index. For a fixed-base system, the first body rigidly fixed to the world is body-0. For a floating-base system, the floating base is body-0.
Generalized Velocity (DOF) Index: All joints are mapped to a specific set of generalized velocity indicies.
Generalized Coordinate Index:
Frame Index:
Conversions Between Indicies¶
A body index to a generalized velocity index:
ArticulatedSystem::getMappingFromBodyIndexToGeneralizedVelocityIndex()
A body index to a generalized coordinate index:
ArticulatedSystem::getMappingFromBodyIndexToGeneralizedCoordinateIndex()
API¶
-
class
raisim
::
ArticulatedSystem
: public raisim::Object¶ Public Functions
-
const raisim::VecDyn &
getGeneralizedCoordinate
() const¶ - Return
generalized coordinate of the system
-
const raisim::VecDyn &
getGeneralizedVelocity
() const¶ - Return
generalized velocity of the system
-
void
getBaseOrientation
(raisim::Vec<4> &quaternion) const¶ - Parameters
[out] quaternion
: orientation of base
-
void
getBaseOrientation
(raisim::Mat<3, 3> &rotataionMatrix) const¶ - Parameters
[out] rotataionMatrix
: orientation of base
-
const raisim::Mat<3, 3> &
getBaseOrientation
() const¶ - Return
orientation of base
-
void
getBasePosition
(raisim::Vec<3> &position) const¶ - Parameters
[out] position
: position of base
-
raisim::Vec<3>
getBasePosition
() const¶ - Return
position of base
-
void
updateKinematics
()¶ unnecessary to call this function if you are simulating your system. integrate1 calls this function Call this function if you want to get kinematic properties but you don’t want to integrate.
-
void
setGeneralizedCoordinate
(const Eigen::VectorXd &jointState)¶ set gc of each joint in order
- Parameters
[in] jointState
: generalized coordinate
-
void
setGeneralizedCoordinate
(const raisim::VecDyn &jointState)¶ set gc of each joint in order
- Parameters
[in] jointState
: generalized coordinate
-
void
setGeneralizedVelocity
(const Eigen::VectorXd &jointVel)¶ set the generalized velocity
- Parameters
[in] jointVel
: the generalized velocity
-
void
setGeneralizedVelocity
(const raisim::VecDyn &jointVel)¶ set the generalized velocity
- Parameters
[in] jointVel
: the generalized velocity
-
void
setGeneralizedCoordinate
(std::initializer_list<double> jointState)¶ set the generalized coordinsate of each joint in order.
- Parameters
[in] jointState
: the generalized coordinate
-
void
setGeneralizedVelocity
(std::initializer_list<double> jointVel)¶ set the generalized velocity of each joint in order
- Parameters
[in] jointVel
: the generalized velocity
-
void
setGeneralizedForce
(std::initializer_list<double> tau)¶ This is feedforward generalized force. In the PD control mode, this differs from the actual generalizedForce the dimension should be the same as dof.
- Parameters
[in] tau
: the generalized force. If the built-in PD controller is active, this force is added to the generalized force from the PD controller
-
void
setGeneralizedForce
(const raisim::VecDyn &tau)¶ This is feedforward generalized force. In the PD control mode, this differs from the actual generalizedForce the dimension should be the same as dof.
- Parameters
[in] tau
: the generalized force. If the built-in PD controller is active, this force is added to the generalized force from the PD controller
-
void
setGeneralizedForce
(const Eigen::VectorXd &tau)¶ This is feedforward generalized force. In the PD control mode, this differs from the actual generalizedForce the dimension should be the same as dof.
- Parameters
[in] tau
: the generalized force. If the built-in PD controller is active, this force is added to the generalized force from the PD controller
-
void
getState
(Eigen::VectorXd &genco, Eigen::VectorXd &genvel) const¶ get both the generalized coordinate and the generalized velocity
- Parameters
[out] genco
: the generalized coordinate[out] genvel
: the generalized velocity
-
void
getState
(VecDyn &genco, VecDyn &genvel) const¶ get both the generalized coordinate and the generalized velocity
- Parameters
[out] genco
: the generalized coordinate[out] genvel
: the generalized velocity
-
void
setState
(const Eigen::VectorXd &genco, const Eigen::VectorXd &genvel)¶ set both the generalized coordinate and the generalized velocity. This updates the kinematics and removes previously computed contact points
- Parameters
[in] genco
: the generalized coordinate[in] genvel
: the generalized velocity
-
const VecDyn &
getGeneralizedForce
() const¶ Generalized force is the sum of (the feedforward force) and (the PD controller force after thresholding by the “effort” specified in the urdf)
- Return
the generalized force
-
const VecDyn &
getFeedForwardGeneralizedForce
() const¶ get the feedfoward generalized force (which is set by the user)
- Return
the feedforward generalized force
-
const MatDyn &
getMassMatrix
() const¶ get the mass matrix
- Return
the mass matrix. Check Object/ArticulatedSystem section in the manual
-
const VecDyn &
getNonlinearities
() const¶ get the coriolis and the gravitational term
- Return
the coriolis and the gravitational term. Check Object/ArticulatedSystem section in the manual
-
const MatDyn &
getInverseMassMatrix
() const¶ get the inverse mass matrix. Note that this is actually damped inverse. It contains the effect of damping and the spring effects
- Return
the inverse mass matrix. Check Object/ArticulatedSystem section in the manual
-
const Vec<3> &
getCompositeCOM
() const¶ get the center of mass of the whole system
- Return
the center of mass
-
const Mat<3, 3> &
getCompositeInertia
() const¶ get the current composite inertia of the whole system. This value assumes all joints are fixed
- Return
the inertia of the coposite system
-
Vec<3>
getLinearMomentum
() const¶ linear momentum of the whole system
- Return
momentum
-
const VecDyn &
getGeneralizedMomentum
() const¶ returns the generalized momentum which is M * u It is already computed in “integrate1()” so you don’t have to compute again.
- Return
the generalized momentum
-
double
getEnergy
(const Vec<3> &gravity) const¶ - Return
the sum of potential/kinetic energy given the gravitational acceleration
- Parameters
[in] gravity
: gravitational acceleration
-
double
getKineticEnergy
() const¶ - Return
the kinetic energy.
-
double
getPotentialEnergy
(const Vec<3> &gravity) const¶ - Return
the potential energy (relative to zero height) given the gravity vector
- Parameters
[in] gravity
: gravitational acceleration
-
void
getAngularMomentum
(const Vec<3> &referencePoint, Vec<3> &angularMomentum) const¶ - Parameters
[in] referencePoint
: the reference point about which the angular momentum is computed[in] angularMomentum
: angular momentum about the reference point
-
void
printOutBodyNamesInOrder
() const¶ bodies here means moving bodies. Fixed bodies are optimized out
-
void
printOutFrameNamesInOrder
() const¶ frames are attached to every joint coordinate
-
const std::vector<std::string> &
getMovableJointNames
() const¶ getMovableJointNames. Note! the order doesn’t correspond to dof since there are joints with multiple dof’s
- Return
movable joint names in the joint order.
-
void
getPosition
(size_t bodyIdx, const Vec<3> &point_B, Vec<3> &point_W) const final¶ - Return
position in the world frame of a point defined in a joint frame
- Parameters
[in] bodyIdx
: The body which contains the point, can be retrieved by getBodyIdx()[in] point_B
: The position of the point in the body frame[out] point_W
: The position of the point in the world frame
-
CoordinateFrame &
getFrameByName
(const std::string &nm)¶ Refer to Object/ArticulatedSystem/Kinematics/Frame in the manual for details
- Return
the coordinate frame of the given name
- Parameters
[in] nm
: name of the frame
-
CoordinateFrame &
getFrameByIdx
(size_t idx)¶ Refer to Object/ArticulatedSystem/Kinematics/Frame in the manual for details
- Return
the coordinate frame of the given index
- Parameters
[in] idx
: index of the frame
-
size_t
getFrameIdxByName
(const std::string &nm) const¶ Refer to Object/ArticulatedSystem/Kinematics/Frame in the manual for details The frame can be retrieved as as->getFrames[index]. This way is more efficient than above methods that use the frame name
- Return
the index of the coordinate frame of the given index.
- Parameters
[in] nm
: name of the frame
-
std::vector<CoordinateFrame> &
getFrames
()¶ Refer to Object/ArticulatedSystem/Kinematics/Frame in the manual for details The frame can be retrieved as as->getFrames[index]. This way is more efficient than above methods that use the frame name
- Return
a vector of the coordinate frames
-
void
getFramePosition
(size_t frameId, Vec<3> &point_W) const¶ - Parameters
[in] frameId
: the frame id which can be obtained by getFrameIdxByName()[out] point_W
: the position of the frame expressed in the world frame
-
void
getFrameOrientation
(size_t frameId, Mat<3, 3> &orientation_W) const¶ - Parameters
[in] frameId
: the frame id which can be obtained by getFrameIdxByName()[out] orientation_W
: the position of the frame relative to the world frame
-
void
getFrameVelocity
(size_t frameId, Vec<3> &vel_W) const¶ - Parameters
[in] frameId
: the frame id which can be obtained by getFrameIdxByName()[out] vel_W
: the linear velocity of the frame expressed in the world frame
-
void
getFrameAngularVelocity
(size_t frameId, Vec<3> &angVel_W) const¶ - Parameters
[in] frameId
: the frame id which can be obtained by getFrameIdxByName()[out] angVel_W
: the angular velocity of the frame expressed in the world frame
-
void
getFramePosition
(const std::string &frameName, Vec<3> &point_W) const¶ - Parameters
[in] frameName
: the frame name (defined in the urdf)[out] point_W
: the position of the frame expressed in the world frame
-
void
getFrameOrientation
(const std::string &frameName, Mat<3, 3> &orientation_W) const¶ - Parameters
[in] frameName
: the frame name (defined in the urdf)[out] orientation_W
: the orientation of the frame relative to the world frame
-
void
getFrameVelocity
(const std::string &frameName, Vec<3> &vel_W) const¶ - Parameters
[in] frameName
: the frame name (defined in the urdf)[out] vel_W
: the linear velocity of the frame expressed in the world frame
-
void
getFrameAngularVelocity
(const std::string &frameName, Vec<3> &angVel_W) const¶ - Parameters
[in] frameName
: the frame name (defined in the urdf)[out] angVel_W
: the angular velocity of the frame relative to the world frame
-
void
getFramePosition
(const CoordinateFrame &frame, Vec<3> &point_W) const¶ - Parameters
[in] frame
: custom frame defined by the user[out] point_W
: the position of the frame relative to the world frame
-
void
getFrameOrientation
(const CoordinateFrame &frame, Mat<3, 3> &orientation_W) const¶ - Parameters
[in] frame
: custom frame defined by the user[out] orientation_W
: the orientation of the frame relative to the world frame
-
void
getFrameVelocity
(const CoordinateFrame &frame, Vec<3> &vel_W) const¶ - Parameters
[in] frame
: custom frame defined by the user[out] vel_W
: the linear velocity of the frame expressed to the world frame
-
void
getFrameAngularVelocity
(const CoordinateFrame &frame, Vec<3> &angVel_W) const¶ - Parameters
[in] frame
: custom frame defined by the user[out] angVel_W
: the angular velocity of the frame expressed to the world frame
-
void
getPosition
(size_t bodyIdx, Vec<3> &pos_w) const final¶ - Parameters
[in] bodyIdx
: the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()[out] pos_w
: the position of the joint (after its own joint transformation)
-
void
getOrientation
(size_t bodyIdx, Mat<3, 3> &rot) const final¶ - Parameters
[in] bodyIdx
: the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()[out] rot
: the orientation of the joint (after its own joint transformation)
-
void
getVelocity
(size_t bodyIdx, Vec<3> &vel_w) const final¶ - Parameters
[in] bodyIdx
: the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()[out] vel_w
: the linear velocity of the joint (after its own joint transformation)
-
void
getAngularVelocity
(size_t bodyIdx, Vec<3> &angVel_w) const¶ - Parameters
[in] bodyIdx
: the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()[out] angVel_w
: the angular velocity of the joint (after its own joint transformation)
-
void
getSparseJacobian
(size_t bodyIdx, const Vec<3> &point_W, SparseJacobian &jaco) const¶ - Parameters
[in] bodyIdx
: the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()[in] point_W
: the point expressed in the world frame. If you want to use a point expressed in the body frame, use getDenseFrameJacobian()[out] jaco
: the positional Jacobian. v = J * u. v is the linear velocity expressed in the world frame and u is the generalized velocity
-
void
getSparseRotationalJacobian
(size_t bodyIdx, SparseJacobian &jaco) const¶ - Parameters
[in] bodyIdx
: the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()[out] jaco
: the rotational Jacobian. = J * u. is the angular velocity expressed in the world frame and u is the generalized velocity
-
void
getDenseJacobian
(size_t bodyIdx, const Vec<3> &point_W, Eigen::MatrixXd &jaco) const¶ This method only fills out non-zero elements. Make sure that the jaco is setZero() once in the initialization!
- Parameters
[in] bodyIdx
: the body index. Note that body index and the joint index are the same because every body has one parent joint[in] point_W
: the point expressed in the world frame. If you want to use a point expressed in the body frame, use getDenseFrameJacobian()[out] jaco
: the dense positional Jacobian
-
void
getDenseRotationalJacobian
(size_t bodyIdx, Eigen::MatrixXd &jaco) const¶ This method only fills out non-zero elements. Make sure that the jaco is setZero() once in the initialization!
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[out] jaco
: the dense rotational Jacobian
-
void
getDenseFrameJacobian
(size_t frameIdx, Eigen::MatrixXd &jaco) const¶ This method only fills out non-zero elements. Make sure that the jaco is setZero() once in the initialization!
- Parameters
[in] frameIdx
: the frame index. it can be retrieved by getFrameIdxByName()[out] jaco
: the dense positional Jacobian
-
void
getDenseFrameJacobian
(const std::string &frameName, Eigen::MatrixXd &jaco) const¶ This method only fills out non-zero elements. Make sure that the jaco is setZero() once in the initialization!
- Parameters
[in] frameName
: the frame name. (defined in the URDF)[out] jaco
: the dense positional Jacobian
-
void
getDenseFrameRotationalJacobian
(size_t frameIdx, Eigen::MatrixXd &jaco) const¶ This method only fills out non-zero elements. Make sure that the jaco is setZero() once in the initialization!
- Parameters
[in] frameIdx
: the frame index. it can be retrieved by getFrameIdxByName()[out] jaco
: the dense rotational Jacobian
-
void
getDenseFrameRotationalJacobian
(const std::string &frameName, Eigen::MatrixXd &jaco) const¶ This method only fills out non-zero elements. Make sure that the jaco is setZero() once in the initialization!
- Parameters
[in] frameName
: the frame name. (defined in the URDF)[out] jaco
: the dense rotational Jacobian
-
void
getVelocity
(const SparseJacobian &jaco, Vec<3> &pointVel) const¶ - Parameters
[in] jaco
: the Jacobian associated with the point of interest[out] pointVel
: the velocity of the point expressed in the world frame
-
void
getVelocity
(size_t bodyIdx, const Vec<3> &posInBodyFrame, Vec<3> &pointVel) const¶ - Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[in] posInBodyFrame
: the position of the point of interest expressed in the body frame[out] pointVel
: the velocity of the point expressed in the world frame
-
size_t
getBodyIdx
(const std::string &nm) const¶ returns the index of the body
- Return
the index of the body
- Parameters
[in] nm
: name of the body. The body name is the name of the movable link of the body
-
size_t
getDOF
() const¶ - Return
the degrees of freedom
-
size_t
getGeneralizedVelocityDim
() const¶ - Return
the dimension of generalized velocity (do the same thing with getDOF)
-
size_t
getGeneralizedCoordinateDim
() const¶ - Return
the dimension of generalized coordinate
-
void
getBodyPose
(size_t bodyIdx, Mat<3, 3> &orientation, Vec<3> &position) const¶ The body pose is the pose of its parent joint (after its joint transformation)
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[out] orientation
: the orientation of the body[out] position
: the position of the body
-
void
getBodyPosition
(size_t bodyIdx, Vec<3> &position) const¶ The body position is the position of its parent joint (after its joint transformation)
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[out] position
: the position of the body
-
void
getBodyOrientation
(size_t bodyIdx, Mat<3, 3> &orientation) const¶ The body orientation is the orientation of its parent joint (after its joint transformation)
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[out] orientation
: the orientation of the body
-
std::vector<raisim::Vec<3>> &
getJointPos_P
()¶ The following 5 methods can be used to directly modify dynamic/kinematic properties of the robot. They are made for dynamic randomization. Use them with caution since they will change the the model permenantly. After you change the dynamic properties, call “void updateMassInfo()” to update some precomputed dynamic properties
- Return
a reference to joint position relative to its parent, expressed in the parent frame.
-
std::vector<raisim::Mat<3, 3>> &
getJointOrientation_P
()¶ - Return
a reference to joint orientation relative to its parent.
-
std::vector<raisim::Vec<3>> &
getJointAxis_P
()¶ - Return
a reference to joint axis relative to its parent, expressed in the parent frame.
-
const raisim::Vec<3> &
getJointAxis
(size_t idx) const¶ - Return
a reference to joint axis expressed in the world frame.
-
std::vector<double> &
getMass
()¶ You MUST call updateMassInfo() after you change the mass
- Return
a reference to mass of each joint.
-
std::vector<raisim::Mat<3, 3>> &
getInertia
()¶ - Return
a reference to inertia of each body.
-
std::vector<raisim::Vec<3>> &
getBodyCOM_B
()¶ - Return
a reference to the position of the center of the mass of each body in the body frame.
-
std::vector<raisim::Vec<3>> &
getBodyCOM_W
()¶ - Return
a reference to the position of the center of the mass of each body in the world frame.
-
raisim::CollisionSet &
getCollisionBodies
()¶ - Return
a reference to the collision bodies. Position and orientation can be set dynamically
-
raisim::CollisionDefinition &
getCollisionBody
(const std::string &name)¶ - Return
a reference to the collision bodies. Position and orientation can be set dynamically
- Parameters
name
: collision body name which is “LINK_NAME” + “/” + “COLLISION_NUMBER”. For example, the first collision body of the link “base” is named as “base/0”
-
void
updateMassInfo
()¶ This method updates the precomputed composite mass. Call this method after you change link mass. This also updates the center of mass without integration
-
double
getMass
(size_t bodyIdx) const final¶ - Return
mass of the body
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()
-
void
setMass
(size_t bodyIdx, double value)¶ set body mass. It is indexed for each body, no each link
-
double
getTotalMass
() const¶ - Return
the total mass of the system
-
void
setExternalForce
(size_t bodyIdx, const Vec<3> &force) final¶ set external forces or torques expressed in the world frame acting on the COM of the body. The external force is applied for a single time step only. You have to apply the force for every time step if you want persistent force
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[in] force
: force applied to the body (at the center of mass)
-
void
setExternalForce
(size_t bodyIdx, Frame frameOfForce, const Vec<3> &force, Frame frameOfPos, const Vec<3> &pos)¶ set external force acting on the point specified The external force is applied for a single time step only. You have to apply the force for every time step if you want persistent force
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[in] frameOfForce
: the frame in which the force is expressed. Options: Frame::WORLD_FRAME, Frame::PARENT_FRAME or Frame::BODY_FRAME[in] force
: the applied force[in] frameOfPos
: the frame in which the position vector is expressed. Options: Frame::WORLD_FRAME, Frame::PARENT_FRAME or Frame::BODY_FRAME[in] pos
: position at which the force is applied
-
void
setExternalForce
(size_t bodyIdx, const Vec<3> &pos, const Vec<3> &force) final¶ set external force (expressed in the world frame) acting on the point (expressed in the body frame) specified The external force is applied for a single time step only. You have to apply the force for every time step if you want persistent force
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[in] force
: the applied force[in] pos
: position at which the force is applied
-
void
setExternalForce
(const std::string &frame_name, const Vec<3> &force)¶ set external force (expressed in the world frame) acting on the point specified by the frame The external force is applied for a single time step only. You have to apply the force for every time step if you want persistent force
- Parameters
[in] frame_name
: the name of the frame where you want to applied the force. The force is applied to the origin of the frame, on the body where the frame is attached to.[in] force
: the applied force in the world frame
-
void
setExternalTorque
(size_t bodyIdx, const Vec<3> &torque_in_world_frame) final¶ set external torque. The external torque is applied for a single time step only. You have to apply the force for every time step if you want persistent torque
- Parameters
[in] bodyIdx
: the body index. it can be retrieved by getBodyIdx()[in] torque_in_world_frame
: the applied torque expressed in the world frame
-
void
getContactPointVel
(size_t contactId, Vec<3> &vel) const final¶ returns the contact point velocity. The contactId is the order in the vector from Object::getContacts()
- Parameters
[in] contactId
: index of the contact vector which can be obtained by getContacts()[out] vel
: the contact point velocity
-
void
setControlMode
(ControlMode::Type mode)¶ - Parameters
[in] mode
: control mode. Can be either ControlMode::FORCE_AND_TORQUE or ControlMode::PD_PLUS_FEEDFORWARD_TORQUE
-
ControlMode::Type
getControlMode
() const¶ - Return
control mode. Can be either ControlMode::FORCE_AND_TORQUE or ControlMode::PD_PLUS_FEEDFORWARD_TORQUE
-
void
setPdTarget
(const Eigen::VectorXd &posTarget, const Eigen::VectorXd &velTarget)¶ set PD targets.
- Parameters
[in] posTarget
: position target[in] velTarget
: velocity target
-
void
getPdTarget
(Eigen::VectorXd &posTarget, Eigen::VectorXd &velTarget)¶ get PD targets.
- Parameters
[out] posTarget
: position target[out] velTarget
: velocity target
-
void
setPdTarget
(const raisim::VecDyn &posTarget, const raisim::VecDyn &velTarget)¶ set PD targets.
- Parameters
[in] posTarget
: position target (dimension == getGeneralizedCoordinateDim())[in] velTarget
: velocity target (dimension == getDOF())
-
template<class
T
>
voidsetPTarget
(const T &posTarget)¶ set P targets.
- Parameters
[in] posTarget
: position target (dimension == getGeneralizedCoordinateDim())
-
template<class
T
>
voidsetDTarget
(const T &velTarget)¶ set D targets.
- Parameters
[in] velTarget
: velocity target (dimension == getDOF())
-
void
setPdGains
(const Eigen::VectorXd &pgain, const Eigen::VectorXd &dgain)¶ set PD gains.
-
void
getPdGains
(Eigen::VectorXd &pgain, Eigen::VectorXd &dgain)¶ get PD gains.
-
void
setPdGains
(const raisim::VecDyn &pgain, const raisim::VecDyn &dgain)¶ set PD gains.
-
template<class
T
>
voidsetPGains
(const T &pgain)¶ set P gain.
- Parameters
[in] pgain
: position gain (dimension == getDOF())
-
template<class
T
>
voidsetDGains
(const T &dgain)¶ set D gains.
- Parameters
[in] dgain
: velocity gain (dimension == getDOF())
-
void
setJointDamping
(const Eigen::VectorXd &dampingCoefficient)¶ passive elements at the joints. They can be specified in the URDF file as well. Check Object/ArticulatedSystem/URDF convention in the manual
- Parameters
[in] dampingCoefficient
: the damping coefficient vector, acting at each degrees of freedom
-
void
setJointDamping
(const raisim::VecDyn &dampingCoefficient)¶ passive elements at the joints. They can be specified in the URDF file as well. Check Object/ArticulatedSystem/URDF convention in the manual
- Parameters
[in] dampingCoefficient
: the damping coefficient vector, acting at each degrees of freedom
-
void
computeSparseInverse
(const MatDyn &M, MatDyn &Minv) noexcept¶ This computes the inverse mass matrix given the mass matrix. The return type is dense. It exploits the sparsity of the mass matrix to efficiently perform the computation. The outcome also contains effects of the joint damping and strings
- Parameters
[in] M
: mass matrix[out] Minv
: inverse mass matrix
-
void
massMatrixVecMul
(const VecDyn &vec1, VecDyn &vec) const¶ this method exploits the sparsity of the mass matrix. If the mass matrix is nearly dense, it will be slower than your ordinary matrix multiplication which is probably vectorized vec = M * vec1
- Parameters
[in] vec
: input vector[out] vec1
: output vector
-
void
ignoreCollisionBetween
(size_t bodyIdx1, size_t bodyIdx2)¶ The bodies specified here will not collide
- Parameters
[in] bodyIdx1
: first body index[in] bodyIdx2
: second body index
-
ArticulatedSystemOption
getOptions
() const¶ Currently only supports “DO_NOT_COLLIDE_WITH_PARENT”
- Return
articulated system option
-
const std::vector<std::string> &
getBodyNames
() const¶ - Return
a vector of body names (following the joint order)
-
std::vector<VisObject> &
getVisOb
()¶ - Return
a vector of visualized bodies
-
std::vector<VisObject> &
getVisColOb
()¶ - Return
a vector of visualized collision bodies
-
void
getVisObPose
(size_t visObjIdx, Mat<3, 3> &rot, Vec<3> &pos) const¶ - Parameters
[in] visObjIdx
: visual object index. Following the order specified by the vector getVisOb()[out] rot
: orientation[out] pos
: position
-
void
getVisColObPose
(size_t visColObjIdx, Mat<3, 3> &rot, Vec<3> &pos) const¶ - Parameters
[in] visColObjIdx
: visual collision object index. Following the order specified by the vector getVisColOb()[out] rot
: orientation[out] pos
: position
-
const std::string &
getResourceDir
() const¶ - Return
the resource directory (for mesh files, textures, etc)
-
const std::string &
getRobotDescriptionfFileName
() const¶ - Return
the resource directory (for mesh files, textures, etc)
-
const std::string &
getRobotDescriptionfTopDirName
() const¶ - Return
the name of the URDF file (returns empty string if the robot was not specified by a URDF file)
-
const std::string &
getRobotDescriptionFullPath
() const¶ - Return
the full path to the URDF file (returns empty string if the robot was not specified by a URDF file)
-
const std::string &
getRobotDescription
() const¶ - Return
if the object was instantiated with raw URDF string, it returns the string
-
void
exportRobotDescriptionToURDF
(const std::string &filePath) const¶ if the object was instantiated with raw URDF string, it exports the robot description to an URDF file
- Parameters
[in] filePath
: Path where the file is generated
-
void
setBasePos_e
(const Eigen::Vector3d &pos)¶ set the base position using an eigen vector
- Parameters
[in] pos
: position of the base
-
void
setBaseOrientation_e
(const Eigen::Matrix3d &rot)¶ set the base orientation using an eigen vector
- Parameters
[in] rot
: orientation of the base
-
void
setBasePos
(const Vec<3> &pos)¶ set the base position using an eigen vector
- Parameters
[in] pos
: position of the base
-
void
setBaseOrientation
(const Mat<3, 3> &rot)¶ set the base orientation using an eigen vector
- Parameters
[in] rot
: orientation of the base
-
void
setActuationLimits
(const Eigen::VectorXd &upper, const Eigen::VectorXd &lower)¶ set limits in actuation force. It can be also specified in the URDF file
- Parameters
[in] upper
: upper joint force/torque limit[in] lower
: lower joint force/torque limit
-
const VecDyn &
getActuationUpperLimits
() const¶ - Return
the upper joint torque/force limit
-
const VecDyn &
getActuationLowerLimits
() const¶ - Return
the lower joint torque/force limit
-
void
setCollisionObjectShapeParameters
(size_t id, const std::vector<double> ¶ms)¶ change collision geom parameters.
- Parameters
[in] id
: collision object id[in] params
: collision object parameters (depending on the object). For a sphere, {raidus}. For a cylinder and a capsule, {radius, length} For a box, {x-dim, y-dim, z-dim}
-
void
setCollisionObjectPositionOffset
(size_t id, const Vec<3> &posOffset)¶ change collision geom offset from the joint position.
- Parameters
[in] id
: collision object id[in] posOffset
: the position vector expressed in the joint frame
-
void
setCollisionObjectOrientationOffset
(size_t id, const Mat<3, 3> &oriOffset)¶ change collision geom orientation offset from the joint frame.
- Parameters
[in] id
: collision object id[in] oriOffset
: the orientation relative to the joint frame
-
void
setRotorInertia
(const VecDyn &rotorInertia)¶ rotor inertia is a term added to the diagonal of the mass matrix. This approximates the rotor inertia. Note that this is not exactly equivalent in dynamics (due to gyroscopic effect). but it is a commonly used approximation. It can also be expressed in the URDF file
- Parameters
[in] rotorInertia
: the rotor inertia
-
const VecDyn &
getRotorInertia
() const¶ rotor inertia is a term added to the diagonal of the mass matrix. This approximates the rotor inertia. Note that this is not exactly equivalent in dynamics (due to gyroscopic effect). but it is a commonly used approximation. It can also be expressed in the URDF file
- Return
the rotor inertia
-
Joint::Type
getJointType
(size_t jointIndex)¶ This joint indices are in the same order as the elements of the generalized velocity However, some joints have multiple degrees of freedom and they are not equal
- Return
the joint type
- Parameters
[in] jointIndex
: the joint index
-
size_t
getNumberOfJoints
() const¶ - Return
the number of joints (same as the number of bodies)
-
JointRef
getJoint
(const std::string &name)¶ returns reference object of the joint
- Return
a JointRef to the joint. Check the example JointRefAndLinkRef
- Parameters
[in] name
: joint name
-
LinkRef
getLink
(const std::string &name)¶ returns reference object of the joint
- Return
a LinkRef to the link. Check the example JointRefAndLinkRef
- Parameters
[in] name
: the link name
-
const std::vector<size_t> &
getMappingFromBodyIndexToGeneralizedVelocityIndex
() const¶ - Return
a mapping that converts body index to gv index
-
const std::vector<size_t> &
getMappingFromBodyIndexToGeneralizedCoordinateIndex
() const¶ - Return
a mapping that converts body index to gv index
-
ObjectType
getObjectType
() const final¶ - Return
the object type (ARTICULATED_SYSTEM)
-
BodyType
getBodyType
(size_t bodyIdx) const final¶ - Return
the body type (STATIC, KINEMATIC, or DYNAMIC) of the specified body. It is always DYNAMIC except for the fixed base
-
BodyType
getBodyType
() const final¶ - Return
the body type (STATIC, KINEMATIC, or DYNAMIC). It is always dynamic
-
void
setIntegrationScheme
(IntegrationScheme scheme)¶ - Parameters
[in] scheme
: the integration scheme. Can be either TRAPEZOID, SEMI_IMPLICIT, EULER, or RUNGE_KUTTA_4. We recommend TRAPEZOID for systems with many collisions. RUNGE_KUTTA_4 is useful for systems with few contacts and when the integration accuracy is important
-
const std::vector<contact::Single3DContactProblem*> &
getJointLimitViolations
()¶ usage example: For 1d joints (e.g., revolute or prismatic), you can get the impulse due to the joint limit as
robot.getJointLimitViolations()[0]->imp_i[0]
For a ball joint, all three components of imp_i represent the torque in the 3d space. The following joint returns the joint/body Idrobot.getJointLimitViolations()[0]->jointId
.- Return
get contact problems associated with violated joint limits
-
void
setJointLimits
(const std::vector<raisim::Vec<2>> &jointLimits)¶ set new joint limits For revolute and prisimatic joints, the joint limit is {lower, upper} For spherical joint, the joint limit is {angle, NOT_USED}
- Parameters
[in] jointLimits
: joint limits
-
const std::vector<raisim::Vec<2>> &
getJointLimits
()¶ get the joint limits For revolute and prisimatic joints, the joint limit is {lower, upper} For spherical joint, the joint limit is {angle, NOT_USED}
- Return
jointLimits joint limits
-
void
clearExternalForcesAndTorques
()¶ Clears all external forces and torques
Public Static Functions
-
void
convertSparseJacobianToDense
(const SparseJacobian &sparseJaco, Eigen::MatrixXd &denseJaco)¶ - Parameters
[in] sparseJaco
: sparse Jacobian (either positional or rotational)[out] denseJaco
: the corresponding dense Jacobian
-
class
JointRef
¶
-
class
LinkRef
¶
-
struct
SpringElement
¶
-
const raisim::VecDyn &