Robots¶

Model objects are used to simulate dynamics and kinematics. Robot classes have been as factories to easily switch between different scenarios.

Robot Model¶

Defines

NORMALIZE_QUATERNIONS¶

Set to true to normalize quaternions inside joint positions before writing to a model. When false no checks are performed.

Keeping this false should be fine most of the time. Small perturbations in a quaternion can be dealt with by the simulator. Note this does not directly modify the optimization variables, only the copied values. Also note note this cannot replace a quaternion constraint.

namespace gambol

class RobotModel¶

#include <RobotModel.h>

Class for robot instances that can be inserted into the optimizer

Extend this class with your own robots. You could wrap an existing simulator or rely on analytical expressions.

Subclassed by gambol::MuJoCoRobotModel, gambol::RaiSimRobotModel

Methods to do quaternion magic

This is focused on quaternions and angular velocities in body-fixed frame, for quaternions like [w, x, y, z].

Note: this is applicable to body-reference Quaternions! for quaternions in fixed-reference, the QuaternionMatrix definition is different.

Vector4d QuaternionDerivative(const Vector4d &quat, const Vector3d &omega) const¶: Get quaternion derivative from angular velocity

virtual MatrixXd QuaternionMatrix(const Vector4d &quat) const¶

Get quaternion in matrix form such that: d quat/dt = 0.5 quat x omega (quaternion multiplication) = 0.5 quat_M * omega (matrix multiplication)

This is \(\tilde{Q_{\omega}}\) in the documentation.

MatrixXd QuaternionMatrixTranspose(const Vector4d &quat) const¶

Get quaternion in matrix form such that: omega = 2 * dquat/dt x q^-1 (quaternion multiplication) = 2 * quat_M * dquat/dt (matrix multiplication)

This is \(\tilde{Q_{\dot{Q}}}\) in the documentation.

virtual MatrixXd AngularVelocityMatrix(Vector3d omega) const¶

Get angular velocity in matrix form such that: d quat/dt = 0.5 quat x omega (quaternion multiplication) = 0.5 * omega_M * quat (matrix multiplication)

This is \(\tilde{\Omega}\) in the documentation.

Public Types

using Ptr = std::shared_ptr<RobotModel>¶

using VectorXd = Eigen::VectorXd¶

using Vector3d = Eigen::Vector3d¶

using Vector4d = Eigen::Vector4d¶

using MatrixXd = Eigen::MatrixXd¶

using Jacobian = Eigen::SparseMatrix<double, Eigen::RowMajor>¶

Public Functions

RobotModel(int size_q, int size_dq, int size_u, int ee_count)¶: Constructor

virtual ~RobotModel() = default¶

virtual RobotModel::Ptr clone() const = 0¶

virtual bool HasIK() const¶

Whether IK is enabled

Not necessary by default

void SetCurrent(const VectorXd &q = VectorXd(0), const VectorXd &dq = VectorXd(0), const VectorXd &u = VectorXd(0), const std::vector<VectorXd> &ee_forces = {})¶: Set current states

virtual void SetCurrent(const NodesHolder &s, int k)¶: Set current states through nodesholder

virtual void SetCurrentTime(const NodesHolder &s, double t)¶: Set current states through nodesholder and time in seconds

int GetEECount() const¶: Return number of end-effectors

virtual const std::vector<int> &GetQuatIndices() const¶: Return vector of quaternion indices

virtual double GetTotalMass() const¶: Return total mass of robot

virtual double GetGravity() const¶: Return gravitational constant

virtual VectorXd GetDynamics() const = 0¶: Get current forward dynamics

virtual Jacobian GetDynamicsJacobianWrtPos() const = 0¶: Get current dynamics jacobian w.r.t. position

virtual Jacobian GetDynamicsJacobianWrtVel() const = 0¶: Get current dynamics jacobian w.r.t. velocity

virtual Jacobian GetDynamicsJacobianWrtTorque() const = 0¶: Get current dynamics jacobian w.r.t. torque

virtual Jacobian GetDynamicsJacobianWrtForces(uint ee_id) const = 0¶: Get current dynamics jacobian w.r.t. forces

virtual VectorXd GetRatesFromVel() const¶: Return joint rates (qpos_dot) from joint velocities (qvel)

virtual Jacobian GetRatesJacobianWrtPos() const¶

Get jacobian of position derivative (qpos_dot) from velocity (qvel), w.r.t. qpos

\[ \dot{q} = B(q) * v \partial \dot{q} / \partial q = \partial (B(q) * v) / \partial q \]

virtual Jacobian GetRatesJacobianWrtVel() const¶

Get jacobian of position derivative (qpos_dot) from velocity (qvel), w.r.t. qvel

Returns the matrix mapping joint velocities to joint rates:

\[ \dot{q} = B(q) * v \partial \dot{q} / \partial v = B(q) \]

virtual Jacobian GetVelJacobianWrtRates() const¶

Get jacobian of velocity (qvel) from joint rates (qpos_dot), w.r.t. qpos_dot

Returns the matrix mapping joint rates to joint velocities:

\[ v = B^-1(q) * \dot{q} \partial v / \partial \dot{q} = B^-1(q) \]

virtual Vector3d GetEEPos(uint ee_id) const = 0¶: Get position in world coordinates of end-effector

virtual Jacobian GetEEPosJacobian(uint ee_id) const = 0¶: Get jacobian of end-effector position to joint positions

virtual Vector3d GetBasePos() const¶: Return position of base in world coordinates

virtual Jacobian GetBasePosJacobian() const¶: Get jacobian of base position w.r.t. joint positions

virtual VectorXd GetInverseKinematics(const Vector3d &base_pos, const Vector4d &base_rot, const std::vector<Vector3d> &ee_pos) const¶: Get body pose through inverse kinematics

virtual void Update() = 0¶: Propagate states through model

Public Static Functions

static Jacobian DenseToSparse(Eigen::MatrixXd mat)¶

Turn dense matrix into a sparse one

This keeps zero values! So no sparsity is used

Protected Attributes

int size_q_¶

int size_dq_¶

int size_u_¶

int ee_count_¶

VectorXd q_¶: Generalized coordinates.

VectorXd dq_¶: Generalized velocity.

VectorXd u_¶: Joint torques (not generalized force)

std::vector<VectorXd> ee_forces_¶: End-effector forces.

std::vector<int> quat_ids_¶

Indices in gen. coordinates that correspond to the start of a quaternion

Can simply be empty if there are no quaternions involved.

MuJoCo Robot Model¶

namespace gambol

class MuJoCoRobotModel : public gambol::RobotModel ¶

#include <MuJoCoRobotModel.h>

Robot model defined through MuJoCo

Use the ee-names to mark the end-effectors. The names must be body names and the body frame must be at the tip of the end-effector.

Public Functions

MuJoCoRobotModel(const std::string &file, int size_q, int size_dq, int size_u, const std::vector<std::string> &ee_names)¶

Constructor

size_q and size_u can also be extracted from the model, however, the parent constructor already requires them.

MuJoCoRobotModel(const MuJoCoRobotModel&) = default¶: Define copy-constructor as default

virtual MuJoCoRobotModel::Ptr clone() const override¶: Clone object

virtual ~MuJoCoRobotModel() = default¶: Destructor

virtual bool HasIK() const override¶: Check if base was defined

virtual double GetTotalMass() const override¶: Return total mass of robot

virtual double GetGravity() const override¶: Return gravitational constant

const std::vector<int> &GetEEBodyIds() const¶: Return vector of MuJoCo body ids of the end-effectors

virtual VectorXd GetDynamics() const override¶: Get current dynamics

virtual Jacobian GetDynamicsJacobianWrtPos() const override¶: Get dynamics jacobian w.r.t position

virtual Jacobian GetDynamicsJacobianWrtVel() const override¶: Get dynamics jacobian w.r.t velocity

virtual Jacobian GetDynamicsJacobianWrtTorque() const override¶: Get dynamics jacobian w.r.t torque

virtual Jacobian GetDynamicsJacobianWrtForces(uint ee_id) const override¶: Get dynamics jacobian w.r.t. forces

virtual Vector3d GetEEPos(uint ee_id) const override¶: Get position of specific end-effector

virtual Jacobian GetEEPosJacobian(uint ee_id) const override¶

Get jacobian of end-effector position to joint positions

Jacobian is the position jacobian:

\[ \dot{p} = J_q * \dot{qpos} \]

virtual Vector3d GetBasePos() const override¶: Get position of base

virtual Jacobian GetBasePosJacobian() const override¶: Get jacobian of base position to joint positions

virtual VectorXd GetInverseKinematics(const Vector3d &base_pos, const Vector4d &base_rot, const std::vector<Vector3d> &ee_pos) const override¶: Solve IK problem

MuJoCoModel &GetMuJoCoModel()¶: Get reference to MuJoCo model

Protected Functions

virtual void Update() override¶: Update internal calculations

Protected Attributes

mutable MuJoCoModel model_¶

MuJoCo model object

mutable since it’s internal states are to be updated, regardless of const flags.

std::vector<int> ee_body_id_¶: Body ids of end-effectors.

int base_body_id_¶: Body id of the base.

RaiSim Robot Model¶

namespace gambol

class RaiSimRobotModel : public gambol::RobotModel ¶

#include <RaiSimRobotModel.h>

Robot model implemented through RaiSim articulated system

The generalized coordinates are called that literally (q, qpos) The generalized velocity is also called exactly that (dq, qvel)

Public Types

using Ptr = std::shared_ptr<RaiSimRobotModel>¶

Public Functions

RaiSimRobotModel(const std::string &urdf_file, int size_q, int size_dq, int size_u, const std::vector<std::string> &ee_names, const Eigen::MatrixXd &S = Eigen::MatrixXd(0, 0))¶

Regular constructor

The end-effectors are defined by fame names (= joint names). It is most convenient to create a type-fixed joint at each end-effector.

See: actuator_selector_

Parameters

urdf_file – Robot model file
size_q –
size_dq –
size_u –
ee_names – List of names of end-effectors
S – Actuator selection matrix,

RaiSimRobotModel(const RaiSimRobotModel&) = delete¶: Copy constructor

~RaiSimRobotModel() override = default¶: Destructor

virtual RobotModel::Ptr clone() const override¶

Clone instance

A pointer of the base class is returned for interchangeability

void SetSelectionMatrix(Eigen::MatrixXd S = Eigen::MatrixXd(0, 0))¶

Set new selection matrix

An error will be thrown if the matrix does not match. Leave to argument empty to try to automatically set a matrix. This default matrix will assume the last #u coordinates are actuated.

See: RaiSimRobotModel::actuator_selector_

virtual VectorXd GetDynamics() const override¶

Get forward dynamics

RaiSim lacks a feature to compute the dynamics and return it, without modifying the model. So instead we perform an integration step, look at the difference and reset the word. This method computes the current dynamics of the current state of this->system_. It is up to the user whether system_ is actually up-to-date with q_, dq_, etc.!

virtual Jacobian GetDynamicsJacobianWrtPos() const override¶

Get current dynamics jacobian w.r.t. position

Calculated through finite difference. This assumes the model and data are up-to-date through SetCurrent().

virtual Jacobian GetDynamicsJacobianWrtVel() const override¶

Get current dynamics jacobian w.r.t. velocity

Calculated through finite difference. This assumes the model and data are up-to-date through SetCurrent().

virtual Jacobian GetDynamicsJacobianWrtTorque() const override¶

Get current dynamics jacobian w.r.t. torque

Calculated through finite difference. This assumes the model and data are up-to-date through SetCurrent().

virtual Jacobian GetDynamicsJacobianWrtForces(uint ee_id) const override¶

Get current dynamics jacobian w.r.t. forces

The end-effector forces are stacked together, so this jacobian has dimensions: ( n_ee * 3 x n_dof )

virtual Vector3d GetEEPos(uint ee_id) const override¶: Get position in world coordinates of end-effector

virtual Jacobian GetEEPosJacobian(uint ee_id) const override¶: Get jacobian of end-effector position to joint positions

void EnableContact(bool enable)¶

Enable or disable collision detection between the robot and its surroundings

Parameters: enable –

raisim::World &GetWorld()¶: Get reference to the raisim world object

raisim::ArticulatedSystem *GetSystem()¶: Get pointer to the raisim system object (the robot itself)

raisim::Ground *GetGround()¶: Get pointer to ground object

std::vector<std::pair<double, double>> GetJointLimits(bool is_3d = false, std::vector<int> *bounds = nullptr)¶

Read joint limits from the system model

The limits are determined by the lower="..." upper="..." clauses. The lower and upper limits of each joint is listed. The size of the returned is list is the number of generalized coordinates. If a joint has no limits, then <-1.0e19, 1.0e19> is filled in. Pass a not-null pointer as an argument to also get a list of bounded joints.

Use is_3d to skip trying to read the first 6 DOF (7 coordinates) and instead consider them not limited.

Parameters

is_3d – Do not consider limits for the first 6 DOFs
[out] bounds – Return a list of the joints that are actually limited.

Returns

List of pairs representing limits

std::vector<std::pair<double, double>> GetActuatorLimits(std::vector<int> *bounds = nullptr)¶

Read actuator limits from the system model

The actuator limits are determined by the effort="..." clauses. The lower and upper limits of each actuator will be listed. The number of actuators size_u is used. Only the nast n joints will then be considered. If an actuator has no limits, then <-1.0e19, 1.0e19> is filled in. Pass a not-null pointer as an argument to also get a list of bounded actuators.

Parameters: [out] bounds – Return a list of the actuators that are actually limited.
Returns: List of pairs representing limits

Protected Functions

virtual void Update() override¶

Propagate states through model

Up until now the information is stored in the object properties, without affecting the simulated world. So we update the world here.

virtual MatrixXd QuaternionMatrix(const Vector4d &quat) const override¶

Get quaternion in matrix form such that: d quat/dt = 0.5 quat x omega (quaternion multiplication) = 0.5 quat_M * omega (matrix multiplication)

This is \(\tilde{Q_{\omega}}\) in the documentation.

virtual MatrixXd AngularVelocityMatrix(Vector3d omega) const override¶

Get angular velocity in matrix form such that: d quat/dt = 0.5 quat x omega (quaternion multiplication) = 0.5 * omega_M * quat (matrix multiplication)

This is \(\tilde{\Omega}\) in the documentation.

const raisim::CoordinateFrame &GetEEFrame(uint ee_id) const¶

Get frame of end-effector

Parameters: ee_id – End-effector id

void SetSystemActuatorTorque(const VectorXd &u) const¶: Set actuator torque in system_

void SetSystemEEForces(const std::vector<VectorXd> &forces) const¶

Set end-effector forces to the raisim system_

Parameters: forces – Set forces as a list of vectors

Protected Attributes

mutable raisim::World world_¶

Raisim simulation world

mutable because updating its information requires writing.

mutable raisim::ArticulatedSystem *system_¶: The actual URDF robot

mutable raisim::Ground *ground_¶: Ground object (needed to save for visualization)

std::string file_¶: URDF file used to create robot

std::vector<std::string> ee_names_¶: End-effector names (needed for clone method)

mutable std::vector<VectorXd> system_ee_forces_¶

External forces on the system, mirror to ee_forces_

A raisim system does not remember external forces. So instead we track our own list of forces that we actually apply at the start of GetDynamics(). This is separate from ee_forces_ to make e.g. the dynamics jacobian easier.

const double eps = 1e-5¶: Finite-difference step.

Eigen::MatrixXd actuator_selector_¶

Joint actuator selection matrix

Used like:

\[ \tau = S_a * \tau_a \]

There is no convenient way of relating joints and actuators, so this explicit matrix is used instead. By default the selection matrix will skip the first 6 DOF if a floating joint is registered. In case more joints are passive, overwrite this matrix.

std::vector<size_t> ee_frame_ids_¶: List of frame ids corresponding to end-effectors

std::vector<const raisim::CoordinateFrame*> ee_frames_¶

List of references to the frames of end-effectors

Pointers actually, because a vector of references is not allowed.

Protected Static Attributes

static bool activated = false¶: Track whether RaiSim activation has been performed at all

Robots¶

namespace gambol

class Robot¶

#include <Robot.h>

Wrapper structure for robots

A Robot instance should make it easy to swap models in the same optimization. However, it should not be essential!

Public Types

enum RobotName¶

Pre-defined robotic systems for optimization.

Values:

enumerator Monoped¶: Monoped, one-legged hopper (free base joint)

enumerator Monoped3D¶: Monoped in 3D (pinfoot)

enumerator Monoped3DEuler¶: Monoped in 3D (pinfoot, with Euler angles)

enumerator MonopedFoot¶: Monoped but with free foot joint instead.

enumerator Biped¶: Two-legged 5-link biped.

enumerator Biped3D¶: Two-legged 5-link biped in 3D (with a freejoint)

enumerator BipedFeet¶: Biped with toes.

enumerator BipedFeet3D¶: Biped with toes in 3D.

enumerator BipedArms3D¶: Biped with arms in 3D.

enumerator ExoSkeleton¶: A basic model of the BME Exo.

enumerator Quadruped¶: Quadruped in 2D.

enumerator Quadruped3D¶: Quadruped in 3D.

enumerator Block¶: A simple legless block.

enumerator CartPole¶: Pendulum on a cart.

enumerator SnakeBot¶: 4-link snake

enumerator RaiSimBlock¶: A simple legless block in RaiSimLib instead of MuJoCo.

enumerator RaiSimCartPole¶: Pendulum on a cart, made in RaiSim.

enumerator RaiSimMonoped¶: Monoped in 2D, made in RaiSim.

enumerator RaiSimBiped¶: Two-legged 5-link biped in RaiSim.

enumerator RaiSimBiped3D¶: Two-legged 5-link biped in 3D (with a floating base) in RaiSim.

enumerator RaiSimWE2¶: WE2 URDF model, in RaiSim.

using VectorXd = Eigen::VectorXd¶

using Vector3d = Eigen::Vector3d¶

using Vector4d = Eigen::Vector4d¶

using Limits = std::vector<std::pair<double, double>>¶

using Bounds = std::vector<int>¶

Public Functions

Robot()¶: Default constructor

Robot(RobotName robot)¶

Constructor for a listed Robot

Parameters: robot – Constructor for pre-defined robots

virtual ~Robot() = default¶: Destructor

Public Members

RobotModel::Ptr robot_model_¶

int size_q_¶

int size_dq_¶

int size_u_¶

int num_ee_¶

Limits joint_pos_limits_¶

Bounds bound_joint_pos_limits_¶

VectorXd basic_joint_pos_¶

Limits torque_limits_¶

Bounds bound_torque_limits_¶

std::vector<std::vector<double>> ee_phase_durations_¶

std::vector<bool> initial_contact_¶

Bounds bound_initial_joint_pos_¶

Bounds bound_initial_joint_vel_¶

Bounds bound_symmetry_joint_pos_¶

bool initial_zpos_¶

GaitGenerator::Ptr generator_¶

Public Static Functions

static void JointLimitsFromMuJoCo(const MuJoCoModel &model, Limits &limits, Bounds &bounds)¶

Read joint limits from xml file

Read joint limits from MuJoCo model

Set both limits and bounds.

static void TorqueLimitsFromMuJoCo(const MuJoCoModel &model, Limits &limits, Bounds &bounds)¶

Read torque limits from xml file

Read torque limits from MuJoCo model

Set both limits and bounds.

Private Functions

void MakeMonoped()¶: Create monoped robot

void MakeMonoped3D()¶

Make monoped robot

Model is in 3D

void MakeMonoped3DEuler()¶

Make monoped robot with Euler angles

Model is in 3D and does not contain quaternions

void MakeMonopedFoot()¶

Create monoped robot

Root node is at the foot

void MakeBiped()¶: Create biped robot model

void MakeBiped3D()¶: Create biped robot model

void MakeBipedFeet()¶: Create biped robot with feet

void MakeBipedFeet3D()¶: Create biped robot with feet in 3D

void MakeBipedArms3D()¶: Create biped robot model with pin feet and arms in 3D

void MakeExoSkeleton()¶: Create robot of the BME exoskeleton

void MakeQuadruped()¶: Create quadruped in 2D robot model

void MakeQuadruped3D()¶: Create quadruped in 3D robot model

void MakeBlock()¶

Make simple legless block to test dynamics

Model is in 3D

void MakeCartPole()¶: Make a 2D pendulum on a cart

void MakeSnakeBot()¶: Create biped robot model

void MakeRaiSimBlock()¶

Make simple legless block to test dynamics

This model is simulated by RaiSim instead of MuJoCo! Model is in 3D

void MakeRaiSimCartPole()¶: Make a 2D pendulum on a cart, made in RaiSim

void MakeRaiSimMonoped()¶: Make a monoped in 2D using RaiSim

void MakeRaiSimBiped()¶: Created five-link biped in 2D, modelled in RaiSim

void MakeRaiSimBiped3D()¶: Create biped robot model in 3D, simulated by RaiSim

void MakeRaiSimWE2()¶: Create biped robot model in 3D, simulated by RaiSim