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¶

Calculate quaternion derivative

Get quaternion derivative from angular velocity

virtual MatrixXd QuaternionMatrix(const Vector4d &quat) const¶

Get 4x3 skew form of quaternion

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 4x3 skew form of quaternion (transpose)

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 4x4 skew form of angular velocity

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 = {})¶

Bring model up-to-date with current time instance

Set current states

virtual void SetCurrent(const NodesHolder &s, int k)¶

Bring model up-to-date with current time instance through holder

Set current states through nodesholder

virtual void SetCurrentTime(const NodesHolder &s, double t)¶

Bring model up-to-date with current time (not node ID)

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 list of ids of quaternions

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)

Get position derivative (qpos_dot) from velocity (qvel)

Returns joint rates \(\dot{q}\) (i.e. derivative of generalized coordinates) based on joint velocity \(v\).

virtual Jacobian GetRatesJacobianWrtPos() const¶

Get jacobian of joint rates w.r.t. position (qpos)

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 joint rates w.r.t. joint velocities (qvel)

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 joint velocities (qvel) w.r.t. to joint rates (qpos_dot)

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¶

Get position in world coordinates of the base

Return position of base

virtual Jacobian GetBasePosJacobian() const¶

Get jacobian of end-effector position to joint positions

Return position of base

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

Get body pose through inverse kinematics

Return inverse kinematics for pose

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

Public Static Functions

static Jacobian DenseToSparse(Eigen::MatrixXd mat)¶

Turn dense matrix into a sparse one (keeping zero values!)

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)¶

Construct model from file

Constructor

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

MuJoCoRobotModel(const MuJoCoRobotModel&) = default¶

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

virtual ~MuJoCoRobotModel() = default¶

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 dynamics

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()¶

Return reference to MuJoCo model

Get reference to MuJoCo model

Protected Functions

virtual void Update() override¶

Propagate states through model, to be executed after SetCurrent()

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))¶

Constructor

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

virtual ~RaiSimRobotModel() = default¶: Destructor

virtual RobotModel::Ptr clone() const override¶

Clone method

Clone instance

A pointer of the base class is returned for interchangeability

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

Write new selection matrix

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 )

Get current dynamics jacobian w.r.t. forces

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: Enable or disable collision detection between the robot and its surroundings.
enable:

raisim::World &GetWorld()¶

Get const reference to RaiSim world

Get reference to the raisim world object

raisim::ArticulatedSystem *GetSystem()¶

Get const pointer to RaiSim system (the robot itself)

Get pointer to the raisim system object

raisim::Ground *GetGround()¶

Get pointer to RaiSim ground object

Get pointer to ground object

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 4x3 skew form of quaternion

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 4x4 skew form of angular velocity

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 belonging to end-effector

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_

Set end-effector forces to the 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