Drake
RigidBodyManipulator Class Reference

This class wraps the spatial vector library (v1) provided by Roy Featherstone on his website: http://users.cecs.anu.edu.au/~roy/spatial/documentation.html. More...

Inheritance diagram for RigidBodyManipulator:
Collaboration diagram for RigidBodyManipulator:

## Public Member Functions

function RigidBodyManipulator (filename, options)
Construct a new rigid body manipulator object with a single (empty) RigidBody (called 'world'), and optionally load a first robot from urdf (see documentation for addRobotFromURDF for details on the inputs). More...

function qdotToV (obj, kinsol)

function vToqdot (obj, kinsol)

function output (obj, t, x, u)
The outputs of this dynamical system are concatenated from each of the sensors. More...

function setTerrain (obj, terrain)
Set the ground height profile which the contact points on the robot may not penetrate. More...

function removeTerrainGeometries (varargin)

function removeTerrainGeometry (obj)

function getTerrainHeight (obj, contact_pos)
Access method for querying the terrain height from the RigidBodyTerrain object assigned to this manipulator. More...

function getB (obj, q, qd)
Note: getB(obj) is ok iff there are no direct feedthrough force elements. More...

function setGravity (obj, grav)

function getRandomConfiguration (obj)

function getZeroConfiguration (obj)

function getInitialState (obj)

function setJointLimits (obj, jl_min, jl_max)

function getGravity (obj, grav)

function computeFrictionForce (model, v)
Note: gradient is with respect to v, not q! More...

function getMexModelPtr (obj)
Please do note use this. More...

function cartesianForceToSpatialForce (obj, kinsol, body_ind, point, force)

Adds a sensor to the RigidBodyManipulator. More...

function compile (model)
After parsing, compute some relevant data structures that will be accessed in the dynamics and visualization. More...

function findJointIndices (model, str)

function findPositionIndices (model, str)
findPositionIndices Returns position indices in the state vector for joints whose name contains a specified string. More...

function findVelocityIndices (model, str)
findJointVelocityIndices Returns velocity indices in the state vector for joints whose name contains a specified string. More...

function findFrameId (model, name, robotnum)

function setForce (model, force_ind, force)

function getBodyOrFrameName (model, body_or_frame_id)

function setParams (model, p)

function getParams (model)

function findJointInd (model, varargin)

function getTerrainContactPoints (obj, body_idx, contact_groups)
terrain_contact_point_struct = getTerrainContactPoints(obj) returns a structure array containing the terrain contact points on all bodies of this manipulator. More...

function getContactShapeGroupNames (varargin)

function getCollisionGeometryGroupNames (model)

function removeCollisionGroups (model, contact_groups, robotnum)
model = removeCollisionGroups(model,contact_groups,robotnum) returns the model with the specified contact groups removed More...

function removeCollisionGroupsExcept (model, contact_groups, robotnum, body_ids)
model = removeCollisionGroups(model,contact_groups,robotnum) returns the model with all contact groups removed except for those specified More...

function parseBodyOrFrameID (obj, body_or_frame, robotnum)
body_idx = parseBodyOrFrameID(obj,body_or_frame) returns the body index or frame id associated with the input. More...

function addCollisionGeometryToBody (obj, body_id, geometry, varargin)

function addGeometryToBody (obj, body_id, geometry, varargin)

function removeShapeFromBody (varargin)

function removeVisualGeometryFromBody (obj, body_id, geometry_name)
Removes all geometry from a given body that match a name. More...

function replaceContactShapesWithCHull (varargin)

function replaceCollisionGeometryWithConvexHull (model, body_indices, varargin)

function drawLCMGLAxes (model, lcmgl, q, body_indices)

function drawLCMGLClosestPoints (model, lcmgl, kinsol, varargin)

function drawLCMGLGravity (model, q, gravity_visual_magnitude)
draws a vector centered at the robot's center of mass having the direction of the gravitational force on the robot. More...

function drawLCMGLForces (model, q, qd, gravity_visual_magnitude)
draws the forces and torques on the robot. More...

function getMass (model, robotnum)
todo: write total_mass to class and simply return it instead of looping every time (since the result is a constant between compiles) More...

function energy (model, x)

function constructCOMFrame (model)

function constructVisualizer (obj, options)

function pdcontrol (sys, Kp, Kd, index)
creates new blocks to implement a PD controller, roughly illustrated by q_d —>[ Kp ]–>(+)–—>[ sys ]----——> yout | - | -——[ Kp,Kd ]<-— More...

function findFixedPoint (obj, x0, u0, options)

Removes links from the specified collision filter group. More...

Adds the specified collision filter groups to the "ignored" list of another collision filter group. More...

function removeFromIgnoredListOfCollisionFilterGroup (model, ignored_collision_fgs, collision_fg_name)
Removes the specified collision filter groups from the "ignored" list of another collision filter group. More...

function surfaceTangents (obj, normal)
% compute tangent vectors, according to the description in the last paragraph of Stewart96, p.2678 More...

function getNumContactPairs (obj)

function unilateralConstraints (obj, x)

function getNumUnilateralConstraints (obj)
Returns the number of unilateral constraints, currently only contains the contact pairs. More...

function getPositionFrame (obj, robotnum)
if robotnum is not specified, then it returns a position frame including all position variables (for all robots) More...

function getVelocityFrame (obj, robotnum)
if robotnum is not specified, then it returns a velocity frame including all velocity variables (for all robots) More...

function getStateFrame (obj, robotnum)
if robotnum is not specified, then it returns a state frame including all state variables (for all robots) More...

function analyticalJacobian (obj, kinsol, base, endEffector, points, rotationType)
NOTE: even though geometricJacobian is mexed, I need kinsol.T in what follows, so I can't use a mex kinsol. More...

function approximateIK (obj, q_seed, q_nom, varargin)
Please refer to the interface of inverseKin The inputs are the same as inverseKin, except current implementation does not accept QuasiStaticConstraint object. More...

function bodyKin (obj, kinsol, body_or_frame_ind, pts)
computes the position of pts (given in the global frame) in the body frame More...

function centerOfMassJacobianDotTimesV (obj, kinsol, robotnum)
computes the quantity Jd * v, where J is the center of mass Jacobian in terms of v, i.e. More...

function centroidalMomentumMatrix (robot, kinsol, robotnum, in_terms_of_qdot)
function [A, dA] = centroidalMomentumMatrix(robot, kinsol, robotnum, in_terms_of_qdot) Computes the centroidal momentum matrix (CMM), i.e. More...

function centroidalMomentumMatrixDotTimesV (obj, kinsol, robotnum)

Changes the root link of the kinematic tree. More...

function collidingPoints (obj, kinsol, points, collision_threshold)
idx = collidingPoints(obj, kinsol, points, collision_threshold) returns the indices of the points in collision with any element in the manipulator More...

function collidingPointsCheckOnly (obj, kinsol, points, collision_threshold)
is_in_collision = collidingPoints(obj, kinsol, points, collision_threshold) returns true if any point in points is in collision with the manipulator More...

function collisionDetectTerrain (obj, xA_in_world)

function collisionRaycast (obj, kinsol, origins, ray_endpoints, use_margins)
function distance = collisionRaycast(obj,kinsol, origin, point_on_ray) More...

function compositeRigidBodyInertias (manipulator, inertias_world, dinertias_world)
Computes composite rigid body inertias expressed in world frame. More...

function contactConstraintDerivatives (obj, normal, kinsol, idxA, idxB, xA, xB, d)

function contactConstraints (obj, kinsol, allow_multiple_contacts, active_collision_options)
function [phi,normal,d,xA,xB,idxA,idxB,mu,n,D,dn,dD] = contactConstraints(obj,kinsol,body_idx) Compute the contact constraints for a manipulator, and relevent bases. More...

function contactConstraintsBV (obj, varargin)
function [phi,B,JB,mu] = contactConstraintsBV(obj,kinsol,allow_multiple_contacts,active_collision_options) a variation on the contactConstraints function that uses a different representation of the friction cone–basis vectors are the extreme rays of the cone (no basis for the normal force) More...

function doKinematics (model, q, v, options, qd_old)
kinsol=doKinematics(model, q, v, options, qd_old) Precomputes information necessary to compute kinematic and dynamic quantities such as Jacobians ( More...

function extractFrameInfo (obj, body_or_frame_index)

function findAncestorBodies (obj, body_index)
findAncestorBodies Find indices of ancestors of a body More...

function findKinematicPath (obj, start_body, end_body, use_mex)
findKinematicPath computes the shortest path from one robot link to another More...

function forwardJacDotTimesV (obj, kinsol, body_or_frame_id, points, rotation_type, base_or_frame_id)
Computes Jd * v, where J is a matrix that maps from the joint velocity vector v to the time derivative of x (x and J of format specified in forwardKinV). More...

function forwardKin (obj, kinsol, body_or_frame_id, points, options)
Transforms points given in a frame identified by body_or_frame_id to a frame identified by options.base_or_frame_id, and also computes a representation of the relative rotation (of type specified by rotation_type), stacked in a matrix x. More...

function geometricJacobian (obj, kinsol, base, end_effector, expressed_in, in_terms_of_qdot)
GEOMETRICJACOBIAN Computes the geometric Jacobian from base to end_effector expressed in frame expressed_in. More...

function geometricJacobianDotTimesV (obj, kinsol, base, end_effector, expressed_in)
geometricJacobianDotTimesV computes the convective term' d/dt(J) * v, where J is a geometric Jacobian and v is the vector of joint velocities across the joints on the path from base to end_effector. More...

function geometricJacobianDotV (obj, kinsol, base, endEffector, expressedIn)
GEOMETRICJACOBIANDOTV computes the 'convective term' d/dt(J) * v, where J is a geometric Jacobian and v is the vector of joint velocities across the same joints that the geometric Jacobian spans. More...

function getCMM (varargin)

function getCMMdA (varargin)

function getCOM (model, kinsol, robotnum, in_terms_of_qdot)

function inertiasInWorldFrame (manipulator, kinsol)
Transforms 6x6 inertia matrices from body frame to world frame. More...

function inverseKin (obj, q_seed, q_nom, varargin)
inverseKin(obj,q_seed,q_nom,constraint1,constraint2,constraint3,,options) % solve IK min_q (q-q_nom)'Q(q-q_nom) subject to constraint1 at t constraint2 at t More...

function inverseKinBackend (obj, mode, t, q_seed, q_nom, varargin)
inverseKin(obj,mode,q_seed,q_nom,t,constraint1,constraint2,,options) % Two modes, mode 1 – solve IK min_q (q-q_nom'Q(q-q_nom) subject to constraint1 at t constraint2 at t More...

function inverseKinPointwise (obj, t, q_seed, q_nom, varargin)
inverseKin(obj,t,q_seed,q_nom,constraint1,constraint2,constraint3,,options) % solve IK at each sample time t(i) seperately min_q(:,i) (q(:,i)-q_nom(:,i))'Q(q(:,i)-q_nom(:,i)) subject to constraint1 at t(i) constraint22 at t(i) More...

function inverseKinTraj (obj, t, q_seed_traj, q_nom_traj, varargin)
inverseKinSequence(obj,t,q_seed_traj,q_nom_traj,constraint1,constraint2,constraint3,,options) % If options.fixInitialState = true solve IK min_q sum_i qdd(:,i)'Qa*qdd(:,i)+qd(:,i)'*Qv*qd(:,i)+(q(:,i)-q_nom(:,i))'*Q(q(:,i)-q_nom(:,i))] subject to constraint1 at t_samples(i) constraint2 at t_samples(i) % constraint(k) at [t_samples(2) t_samples(3) t_samples(nT)] % constraint(k+1) at [t_samples(2) t_samples(3) t_samples(nT)] % . More...

function inverseKinWrapup (obj, q0, varargin)
[q_seed,q_nom,constraint,ikoptions] = inverseKinWrapup(obj,q0, body1,body1_pts,body1_pos, % body1,body2_pts,body2_pos, % body1,body3_pts,body3_pos, % % bodyN,bodyN_pts,bodyN_pos, % options use_mex_constraint) The input to the function is the same input list used in the deprecated IK call, except the last one, which determines whether to construct the constraint in mex only or not. More...

function isBodyPartOfRobot (model, body, robotnum)
determines whether a given body is part of a robot specified by robotnum. More...

function relativeTransform (obj, kinsol, base, body)
Computes the transform that maps vectors from body to base. More...

function spatialAccelerations (obj, kinsol, vd, root_accel)
Computes the spatial accelerations (time derivatives of twists) of all bodies in the RigidBodyManipulator, expressed in world. More...

function terrainContactJacobianDotTimesV (obj, kinsol, varargin)
Computes dJ/dt * v, where J is the matrix that maps the velocity vector v to a stacked vector of terrain contact point velocities. More...

function terrainContactPositions (obj, kinsol, varargin)
x_in_world = terrainContactPositions(obj,kinsol) returns the world-frame position of all terrain contact points on the manipulator. More...

function twists (obj, transforms, q, v)

Public Member Functions inherited from Manipulator
function Manipulator (num_q, num_u, num_v)

virtual function manipulatorDynamics (obj, q, v)
H(q)vdot + C(q,v,f_ext) = Bu. More...

function manipulatorEquations (obj, q, v)
extract the alternative form of the manipulator equations: H(q)vdot + C(q,v)v + G(q) = B(q)u More...

function dynamics (obj, t, x, u)
Provides the DrakeSystem interface to the manipulatorDynamics. More...

function qdotToV (obj, kinsol)
defines the linear map v = Vq * qdot default relationship is that v = qdot More...

function vToqdot (obj, kinsol)
defines the linear map qdot = Vqinv * v default relationship is that v = qdot More...

Adds a position constraint of the form phi(q) = constant which can be enforced directly in the manipulator dynamics. More...

function updatePositionEqualityConstraint (obj, id, con, position_ind)

Adds a velocity constraint of the form psi(q,v) = constant (with dpsidv~=0) which can be enforced directly in the manipulator dynamics. More...

function updateVelocityEqualityConstraint (obj, id, con, velocity_ind)

function removeAllStateConstraints (obj)

function setNumDOF (obj, num)

function getNumDOF (obj)

function setNumPositions (obj, num_q)
Guards the num_positions variable to make sure it stays consistent with num_x. More...

function getNumPositions (obj)

function setNumVelocities (obj, num_v)
Guards the num_velocities variable to make sure it stays consistent with num_x. More...

function getNumVelocities (obj)

function positionConstraints (obj, q)
Implements position constraints of the form phi(q) = 0. More...

function velocityConstraints (obj, q, v)
Implements velocity constraints of the form psi(q,v) = 0 Note: dphidv must not be zero. More...

function getNumJointLimitConstraints (obj)
returns number of constraints imposed by finite joint limits More...

function jointLimitConstraints (obj, q)
constraint function (with derivatives) to implement unilateral constraints imposed by joint limits More...

function num_contacts (obj)

function getNumContacts (obj)

function feedback (sys1, sys2)
Attempt to produce a new manipulator system if possible. More...

Attempt to produce a new manipulator system if possible. More...

function extractTrigPolySystem (obj, options)
Creates a (rational) polynomial system representation of the dynamics. More...

function pdcontrol (sys, Kp, Kd, index)
creates new blocks to implement a PD controller, roughly illustrated by q_d —>[ Kp ]–>(+)–—>[ sys ]----——> yout | - | -——[ Kp,Kd ]<-— More...

function getJointLimits (obj)
Returns lower and upper joint limit vectors. More...

function setJointLimits (obj, jl_min, jl_max)

function doKinematics (q, v)

function parameterEstimation (obj, data, varargin)
Parameter estimation algorithm for manipulators. More...

Public Member Functions inherited from DrakeSystem
function DrakeSystem (num_xc, num_xd, num_u, num_y, direct_feedthrough_flag, time_invariant_flag)
Construct a DrakeSystem. More...

function update (obj, t, x, u)
Placeholder for the update method. More...

function zeroCrossings (obj, t, x, u)
Placeholder for the zeroCrossings method: a method phi = zeroCrossings(t,x,u) which triggers a zero crossing event when phi transitions from positive to negative. More...

function getNumContStates (obj)
Returns the number of continuous states. More...

function getNumDiscStates (obj)
Returns the number of discrete states. More...

function getNumInputs (obj)
Returns the number of inputs to the system. More...

function getNumOutputs (obj)
Returns the number of outputs from the system. More...

function getInitialStateWInput (obj, t, x, u)
Hook in case a system needs to initial state based on current time and/or input. More...

function getSampleTime (obj)
As described at http://www.mathworks.com/help/toolbox/simulink/sfg/f6-58760.html to set multiple sample times, specify one column for each sample time/offset pair. More...

function getInputSampleTimes (obj)
Returns getSampleTime - a DrakeSystem can only have a single same time associated with it. More...

function getOutputSampleTimes (obj)
Returns getSampleTime - a DrakeSystem can only have a single same time associated with it. More...

function setSampleTime (obj, ts)
robust method for setting default sample time More...

function isDirectFeedthrough (obj)
Check if the system is direct feedthrough (e.g., if the output depends on the immediate input) More...

function setDirectFeedthrough (obj, tf)
Set the direct feedthrough flag. More...

function getModel (obj)
Constructs a simulink system block for this system to be used by the simulink engine. More...

function findFixedPoint (obj, x0, u0)
attempts to find a fixed point (xstar,ustar) which also satisfies the constraints, using (x0,u0) as the initial guess. More...

function getDefaultInput (obj)
Define the default initial input so that behavior is well-defined if no controller is specified or if no control messages have been received yet. More...

function setNumContStates (obj, num_xc)
Guards the num_states variable. More...

function setNumDiscStates (obj, num_xd)
Guards the num_states variable. More...

function setNumInputs (obj, num_u)
Guards the num_u variable. More...

function setInputLimits (obj, umin, umax)
Guards the input limits to make sure it stay consistent. More...

function setNumOutputs (obj, num_y)
Guards the number of outputs to make sure it's consistent. More...

function getNumZeroCrossings (obj)
Returns the number of zero crossings. More...

function setNumZeroCrossings (obj, num_zcs)
Guards the number of zero crossings to make sure it's valid. More...

function getNumStateConstraints (obj)

function getNumUnilateralConstraints (obj)

function updateStateConstraint (obj, id, con, xind)

function displayStateConstraints (obj)

function removeAllStateConstraints (obj)

adds state constraints and unilateral constraints to the program on the specified indices. More...

add bounding box constraint todo: consider whether it makes sense to a list of constraints objects instead of just input limits. More...

function stateConstraints (obj, x)
Provides the old interface of a single constraint function which evaluates all of the equality constraints on the state (which should be == 0) More...

function linearize (obj, t0, x0, u0)
Uses the geval engine to linearize the model around the nominal point, at least for the simple case. More...

function simulate (obj, varargin)

function simulateODE (obj, tspan, x0, options)
Simulates the system using the ODE45 suite of solvers instead of the simulink solvers. More...

function feedback (sys1, sys2)
Constructs a feedback combination of sys1 and sys2. More...

Constructs a cascade combination of sys1 and sys2. More...

function extractPolynomialSystem (obj)
Attempts to symbolically extract the extra structure of a polynomial system from the Drake system Will throw an error if the system is not truly polynomial. More...

function extractAffineSystem (obj)
Attempts to symbolically extract the extra structure of an affine system from the Drake system Will throw an error if the system is not truly affine. More...

function extractLinearSystem (obj)
Attempts to symbolically extract the extra structure of a linear system from the Drake system Will throw an error if the system is not truly linear. More...

function makeTrigPolySystem (obj, options)

function systemGradTest (obj, t, x, u, options)
Compare numerical and analytical derivatives of dynamics,update,and output. More...

function extractAffineSystem (obj)

function extractTrigPolySystem (sys, options)
tries to convert the current system into a trig-poly system More...

function taylorApprox (sys, varargin)
performs a taylorApproximation around a point or trajectory usage: taylorApprox(sys,t0,x0,u0,order[,ignores]) or taylorApprox(sys,x0traj,u0traj,order[,ignores]) it returns a polynomial system (or polynomial trajectory system) More...

function transverseLQR (obj, xtraj, utraj, Q, R, Qf, transSurf)
Compute LQR control for transversal system. More...

function transverseLQRClosedLoop (obj, xtraj, utraj, Q, R, Vf, transSurf)

function tvlqr (obj, xtraj, utraj, Q, R, Qf, options)
implements the time-varying linear (or affine) quadratic regulator More...

Public Member Functions inherited from DynamicalSystem
function DynamicalSystem ()

function inInputFrame (sys, frame)
Ensures that sys has the specified input frame, by searching for and cascading a coordinate transformation to the existing input if necessary. More...

function inStateFrame (sys, frame)
Ensures that sys has the specified state frame. More...

function inOutputFrame (sys, frame)
Ensures that sys has the specified output frame, by searching for and cascading a coordinate transformation to the existing output if necessary. More...

Creates a new system with the output of system 1 connected to the input of system 2. More...

function feedback (sys1, sys2)
Creates a new systems with sys1 and sys2 in a feedback interconnect (with sys1 on the forward path, and sys2 on the return path). More...

function parallel (sys1, sys2)
Creates a new system that takes the inputs to both sys1 and sys2 as a single input (which is "demux"ed and passed independently to the two systems), and outputs the "mux"ed output of the two systems. More...

function sampledData (sys, tsin, tsout)
Creates a new system which is a sampled data (e.g. More...

function resolveConstraints (obj, x0, v, constraints)
attempts to find a x which satisfies the constraints, using x0 as the initial guess. More...

adds state constraints and unilateral constriants to the program on the specified indices. More...

function getNumStates (obj)
Returns the total number of states (discrete + continuous) in the system. More...

function getInputSampleTimes (obj)
Returns the sample time of the input See getSampleTime for more details about sample times. More...

function getOutputSampleTimes (obj)
Returns the sample time of the output See getSampleTime for more details about sample times. More...

function isDT (obj)
Returns true if the system has no states, or has states and only one sample time [a b], with a>0. More...

function isCT (obj)
Returns true if the system has only one sample time [a b], with a==0. More...

function isInheritedTime (obj)
Returns true if the system has only one sample time [a b], with b==-1. More...

function isTI (obj)
Returns true if the system is time-invariant. More...

function setTIFlag (obj, bval)
Sets the time invariant flag. More...

function getInputFrame (obj)
Access the CoordinateFrame object that defines the input to this system. More...

function setInputFrame (obj, fr)
Set the CoordinateFrame object that defines the input of this system. More...

function getStateFrame (obj)
Access the CoordinateFrame object that defines the state of this system. More...

function setStateFrame (obj, fr)
Set the CoordinateFrame object that defines the state of this system. More...

function getOutputFrame (obj)
Access the CoordinateFrame object that defines the output to this system. More...

function setOutputFrame (obj, fr)
Set the CoordinateFrame object that defines the output of this system. More...

function getNumStateConstraints (obj)
Returns the scalar number of state constraints (of the form phi(x)=0) More...

function stateConstraints (obj, x)
defines state equality constraints in the form phi(x)=0 More...

function unilateralConstraints (obj, x)
defines state unilateral constraints in the form phi(x)>=0 More...

function getNumUnilateralConstraints (obj)
Returns the scalar number of state constraints (of the form phi(x)>=0) More...

function linearize (obj, t0, x0, u0)
Linearize the system about an operating point (continuous time) More...

function dlinearize (obj, ts, t0, x0, u0)
Linearize the system about an operating point (discrete time) More...

function setParams (obj, p)
This default setParams method attempts to set class properties of the system according to the coordinate names in the parameter frame. More...

function getParams (obj)
This default getParams method attempts to get class properties of the system according to the coordinate names in the parameter frame. More...

function getNumParams (obj)

function setParamFrame (obj, fr)
Set the CoordinateFrame object which describes any system parameters. More...

function getParamFrame (obj)
Returns the CoordinateFrame object which describes any system parameters. More...

function setParamLimits (obj, pmin, pmax)
Set lower and upper bounds on the system parameters. More...

function getParamLimits (obj)
Returns the current lower and upper bounds on the system parameters. More...

function parameterEstimation (obj, data, options)
Estimate parameter values from input-output data. More...

function runLCM (obj, x0, options)
Runs the system as an lcm node. More...

function simulate (obj, tspan, x0, options)
Simulates the dynamical system (using the simulink solvers) More...

function tiHinf (obj, x0, u0, Q, R, Bw, gamma)
Suboptimal state-feedback H-infinity controller for LTI system with bound gamma. More...

function tilqr (obj, x0, u0, Q, R, options)
Computes an LQR controller to stabilize the system around (x0,u0) More...

function tilyap (obj, x0, Q)
Linearizes the system about the state x0 and returns a candidate LyapunovFunction generated by solving the quadratic Lyapunov equation. More...

function tvlyap (obj, xtraj, Q, Qf)
Linearizes the system about the state trajectory xtraj and returns a candidate LyapunovFunction generated by integrating backwards the quadratic differential Lyapunov equation. More...

## Static Public Member Functions

static function parseContactOptions (options)
contact_options = parseContactOptions(options) returns a struct containing settings for contact/collision detection. More...

## Public Attributes

Property B

Property mex_model_ptr

Property dirty

Property collision_filter_groups

Public Attributes inherited from DynamicalSystem
Property warning_manager

## Protected Member Functions

function checkDirty (obj)

function createMexPointer (obj)

function constructStateFrame (model)

function constructParamFrame (model)

function constructInputFrame (model)
inputparents is an array of the parent RigidBodies of each joint. More...

model = adjustCollisionGeometry(model) returns the model with adjusted collision geometry, according to the setings in model.contact_options. More...

Protected Member Functions inherited from DynamicalSystem
function stateIndicesForCombination (sys1, sys2)
Helper method to figure out the indices of the discrete and continuous states after sys1 and sys2 have been combined into a single system. More...

function stateVectorToStructure (obj, xv, mdl)
Converts the vector state xv to the structure xs for simulink state. More...

function stateStructureToVector (obj, xs)
Converts the simulink state structure representation back to the vector state. More...

## Protected Attributes

Property name
names of the objects in the rigid body system More...

Property urdf
names of the urdf files loaded More...

Property body
array of RigidBody objects More...

Property actuator
array of RigidBodyActuator objects More...

Property param_db
cell of structures (one per robot) containing parsed parameter info More...

Property loop
array of RigidBodyLoop objects More...

Property sensor
cell array of RigidBodySensor objects More...

Property force
cell array of RigidBodyForceElement objects More...

Property gravity
note: need {} for arrays that will have multiple types (e.g. More...

Property dim

Property terrain

Property num_contact_pairs

Property contact_options

Property contact_constraint_id

Property cached_terrain_contact_points_struct
struct containing the output of 'obj.getTerrainContactPoints()'. More...

Property quat_norm_constraint_id
indices of state constraints asserting that the quaternion coordinates have norm=1 More...

default kinematics caches; temporary way to make it easy to avoid creating and destroying DrakeMexPointers to KinematicsCache every time doKinematics is called More...

Property frame
array of RigidBodyFrame objects More...

Property robot_state_frames

Property robot_position_frames

Property robot_velocity_frames

## Kinematic Tree

function getNumBodies (obj)

str = getLinkName(obj,body_ind) returns a string containing the link name the specified body if body_ind is a scalar. More...

function addJoint (model, name, type, parent_ind, child_ind, xyz, rpy, axis, damping, coulomb_friction, static_friction, coulomb_window, limits)

note that the case matters. More...

function getBody (model, body_ind)

function setBody (model, body_ind, body)

function getFrame (model, frame_id)

function setFrame (model, frame_id, frame)

function weldJoint (model, body_ind_or_joint_name, robot)

function findJointId (model, jointname, robot_num, error_level)

function leastCommonAncestor (model, body1, body2)
recursively searches for the lowest body in the tree that is an ancestor to both body1 and body2 More...

function drawKinematicTree (model)
depends on having graphviz installed todo: make that a dependency in configure? More...

function newBody (model)

function getActuatedJoints (model)

function removeFixedJoints (model)
takes any fixed joints out of the tree, adding their visuals and inertia to their parents, and connecting their children directly to their parents More...

## Deprecated

function findJoint (model, jointname, robot)

## URDF Parsing

function parseParamString (model, robotnum, str)
Parses parameter strings from URDFs and returns either the value or a msspoly expression for later use with RigidBodyElement.bindParams(). More...

function addRobotFromURDF (model, urdf_filename, xyz, rpy, options)
Parses URDF. More...

Parses URDF from a string. More...

function parseMaterial (node, options)

## SDF Parsing

function addRobotFromSDF (model, sdf_filename, xyz, rpy, options)
Parses SDF and/or Gazebo WORLD file The spec is here: http://sdformat.org/spec A nice trove of models is here: https://bitbucket.org/osrf/gazebo_models/. More...

## Collision

function allCollisions (obj, kinsol)
[ptsA,ptsB,idxA,idxB] = allCollisions(obj,kinsol) uses Bullet's collision checking (including their broad-phase) to give a fast report of all points of collision in the manipulator with the following exceptions: More...

function closestDistance (obj, kinsol, varargin)
function [phi,Jphi] = closestDistance(obj,kinsol,active_collision_options) More...

function collisionDetect (obj, kinsol, allow_multiple_contacts, active_collision_options)
[distance,normal,xA,xB,idxA,idxB] = collisionDetect(obj,kinsol) returns the points of closest approach between all pairs of collision elements in the manipulator, with the following exceptions: More...

function collisionDetectFromPoints (obj, kinsol, points, use_margins)
Given a set of points in global frame, returns the signed distance of each point to the closest surface of the RBM. More...

function pairwiseClosestDistance (obj, kinsol, bodyA_idx, bodyB_idx)
Uses bullet to find the minimum distance between bodies A and B. More...

function pairwiseClosestPoints (obj, kinsol, bodyA_idx, bodyB_idx)
Uses bullet to find the points of closest approach between bodies A and B. More...

function pairwiseContactConstraints (obj, kinsol, body_indA, body_indB, body_collision_indA)
Uses bullet to perform collision detection between rigid body A and rigid body B, and returns a single penetration constraint. More...

function pairwiseContactConstraintsBV (obj, kinsol, body_indA, body_indB, body_collision_indA)
Uses bullet to perform collision detection between rigid body A and rigid body B, and returns a single penetration constraint. More...

function pairwiseContactDistance (obj, kinsol, body_indA, body_indB)
Uses bullet to perform collision detection between rigid body A and rigid body B, and returns a single penetration constraint. More...

function pairwiseContactTest (obj, kinsol, body_indA, body_indB, body_collision_indA)
Uses bullet to perform collision detection between rigid body A and rigid body B. More...

## Detailed Description

This class wraps the spatial vector library (v1) provided by Roy Featherstone on his website: http://users.cecs.anu.edu.au/~roy/spatial/documentation.html.

## Constructor & Destructor Documentation

 function RigidBodyManipulator ( filename , options )

Construct a new rigid body manipulator object with a single (empty) RigidBody (called 'world'), and optionally load a first robot from urdf (see documentation for addRobotFromURDF for details on the inputs).

Return values
 obj

## Member Function Documentation

 function addCollisionGeometryToBody ( obj , body_id , geometry , varargin )

obj must be re-compiled after calling this method

Parameters
 obj - RigidBodyManipulator object body_id - Body index or body name geometry - RigidBodyGeometry (or child class) object group_name - String containing the name of the collision group (optional) Default: 'default'
Return values
 obj
Return values
 obj
 function addFloatingBase ( model , parent , rootlink , xyz , rpy , joint_type )

note that the case matters.

use lower case for extrinsic (absolute) rotations, and upper case for intrinsic (relative) rotations

Return values
 model
 function addFrame ( model , frame )
Return values
 model frame_id
 function addGeometryToBody ( obj , body_id , geometry , varargin )

Parameters
 obj - RigidBodyManipulator object body_id - Body index or body name geometry - RigidBodyGeometry (or child class) object group_name - String containing the name of the collision group (optional) Default: 'default'
Return values
 obj
 function addJoint ( model , name , type , parent_ind , child_ind , xyz , rpy , axis , damping , coulomb_friction , static_friction , coulomb_window , limits )
Return values
 model
Return values
 model id

Parameters
Return values
 model
 function addRobotFromSDF ( model , sdf_filename , xyz , rpy , options )

Parses SDF and/or Gazebo WORLD file The spec is here: http://sdformat.org/spec A nice trove of models is here: https://bitbucket.org/osrf/gazebo_models/.

Parameters
 sdf_filename filename of file to parse
Options:
 floating boolean where true means that a floating joint is automatically added to the root. (note that the default is different than for urdf) Default: true inertial boolean where true means parse dynamics parameters, false means skip them. Default: true visual boolean where true means parse graphics parameters, false means skip them. Default: true Useful for extracting the 2D geometries later. Default: false
Return values
 model
 function addRobotFromURDF ( model , urdf_filename , xyz , rpy , options )

Parses URDF.

Parameters
 urdf_filename filename of file to parse
Options:
 floating boolean where true means that a floating joint is automatically added to the root. Default: false inertial boolean where true means parse dynamics parameters, false means skip them. Default: true visual boolean where true means parse graphics parameters, false means skip them. Default: true visual_geometry boolean where true means to extract the points from the visual geometries (might be very dense for meshes). Useful for extracting the 2D geometries later. Default: false
Return values
 model
 function addRobotFromURDFString ( model , urdf_string , varargin )

Parses URDF from a string.

Filename references must use the "package://" syntax, as there is no notion of a relative path here.

Parameters
 urdf_string xml string containing the urdf
Options:
 floating boolean where true means that a floating joint is automatically added to the root. Default: false inertial boolean where true means parse dynamics parameters, false means skip them. Default: true visual boolean where true means parse graphics parameters, false means skip them. Default: true visual_geometry boolean where true means to extract the points from the visual geometries (might be very dense for meshes). Useful for extracting the 2D geometries later. Default: false
Return values
 model
 function addSensor ( model , sensor )

Adds a sensor to the RigidBodyManipulator.

This modifies the model.sensor parameter and marks the model as dirty.

Parameters
 model existing RigidBodyManipulator the sensor should be added to sensor sensor to add
Return values
 new RigidBodyManipulator with the sensor added.
Return values
 varargout
Return values
 obj
Return values
 obj
 function addToIgnoredListOfCollisionFilterGroup ( model , ignored_collision_fgs , collision_fg_name )

Adds the specified collision filter groups to the "ignored" list of another collision filter group.

Parameters
 ignored_collision_fgs Cell array of strings containing the names of the collision filter groups to be ignored. collision_fg_name String giving the name of the collision filter group that should ignore the groups specified by ignored_collision_fgs
Return values
 model
 function addVisualGeometryToBody ( obj , body_id , geometry )

Parameters
 obj - RigidBodyManipulator object body_id - Body index or body name geometry - RigidBodyGeometry (or child class) object
Return values
 obj
Return values
 varargout
protected

model = adjustCollisionGeometry(model) returns the model with adjusted collision geometry, according to the setings in model.contact_options.

These are

• Replace cylinders with capsules
Parameters
 model - RigidBodyManipulator object
Return values
 model - RigidBodyManipulator object
protected
Return values
 obj
 function allCollisions ( obj , kinsol )

[ptsA,ptsB,idxA,idxB] = allCollisions(obj,kinsol) uses Bullet's collision checking (including their broad-phase) to give a fast report of all points of collision in the manipulator with the following exceptions:

• any body and its parent in the kinematic tree (unless its parent is the world)
• body A and body B, where body A belongs to collision filter groups that ignore all of the collision filter groups to which body B belongs and vice versa

One probably shouldn't use this inside any optimization routines - it returns a variable number of points and has not been checked for numerical differentiability.

Parameters
 kinsol the output of doKinematics. Note that this method requires a kinsol with mex enabled
Return values
 ptsA the i-th column contains the collision point (as identified by Bullet) on body A (in world coordinates) ptsB the i-th column contains the collision point (as identified by Bullet) on body B (in world coordinates) idxA link indices for the first body in each collision idxB link indices for the second body in each collision
 function analyticalJacobian ( obj , kinsol , base , endEffector , points , rotationType )

NOTE: even though geometricJacobian is mexed, I need kinsol.T in what follows, so I can't use a mex kinsol.

Return values
 JAnalytical
 function approximateIK ( obj , q_seed , q_nom , varargin )

Please refer to the interface of inverseKin The inputs are the same as inverseKin, except current implementation does not accept QuasiStaticConstraint object.

Return values
 q the IK solution posture info 0 on sucess, 1 on failure
 function bodyKin ( obj , kinsol , body_or_frame_ind , pts )

computes the position of pts (given in the global frame) in the body frame

Parameters
 kinsol solution structure obtained from doKinematics body_or_frame_ind,an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkId or findFrameInd)
Return values
 x the position of pts (given in the global frame) in the body frame J the Jacobian, dxdq P the gradient, dxdpts - useful when computing forces dJ the gradients of the Jacobian, dJdq

if pts is a 3xm matrix, then x will be a 3xm matrix and (following our gradient convention) J will be a ((3xm)x(q)) matrix, with [J1;J2;;Jm] where Ji = dxidq, P will be a % ((3xm)x(3xm)) matrix, with [P1;P2;] where Pi = dxidpts, and dJ % will be a ((3*m)x(nq^2)) matrix, with [dJ1,dJ2,,dJq] where % dJj = dJdqj

 function cartesianForceToSpatialForce ( obj , kinsol , body_ind , point , force )
Parameters
 body_ind is an index of the body point is a point on the rigid body (in body coords) force is a cartesion force (in world coords)
Return values
 f dfdq dfdforce
 function centerOfMassJacobianDotTimesV ( obj , kinsol , robotnum )

computes the quantity Jd * v, where J is the center of mass Jacobian in terms of v, i.e.

the matrix that maps v to CoM velocity

centroidalMOmentumMatrixDotTimesV
Parameters
 kinsol solution structure obtained from doKinematics robotnum an int array. Jdot * v for the bodies that belong to robot(robotnum) is computed. Default is 1.
Return values
 com_Jacobian_dot_times_v Jdot * v dcom_Jacobian_dot_times_v gradient with respect to q
 function centroidalMomentumMatrix ( robot , kinsol , robotnum , in_terms_of_qdot )

function [A, dA] = centroidalMomentumMatrix(robot, kinsol, robotnum, in_terms_of_qdot) Computes the centroidal momentum matrix (CMM), i.e.

the matrix that maps the joint velocity vector v to the robot's momentum h = [k; l], with angular momentum k and linear momentum l, expressed in a centroidal frame. See Orin13.

Parameters
 kinsol solution structure obtained from doKinematics
Return values
 A CMM dA gradient of A with respect to coordinate vector q
 function centroidalMomentumMatrixDotTimesV ( obj , kinsol , robotnum )
Return values
 function changeRootLink ( rbm , link , xyz , rpy , options )

Changes the root link of the kinematic tree.

Note that the root link is static in the world.

Parameters
rbma RigidBodyManipulator object
Options:
 floating_base (default false)
As always, any leaf nodes with zero inertia will be automatically removed. This will have the intended effect of removing any vestigial floating base joints from the original model.

Example: attach the foot of a floating base manipulator to the ground (to be fleshed out with the details) rbm = addLink(rbm, 'ground'); rbm = addJoint(rbm,'foot','ground', ); % rbm = changeRootLink(rbm,'ground');

Return values
 new_rbm
 function checkDirty ( obj )
protected
 function closestDistance ( obj , kinsol , varargin )

function [phi,Jphi] = closestDistance(obj,kinsol,active_collision_options)

Uses bullet to find the minimum distance between all pairs of collision elements in the subset of the manipulator's collision elements specified by the user, with the following exceptions:

• any body and it's % parent in the kinematic tree
• body A and body B, where body A belongs to collision filter groups that ignore all of the collision filter groups to which body B belongs
Parameters
 kinsol the output from doKinematics() active_collision_options - Struct that may the following fields body_idx - vector of body indices. Only these bodies will be belonging to these groups will be considered for collision detection. Note that the filtering based on collision_filter_groups and adjacency in the kinematic tree still apply. Default: - Consider all bodies and all groups
Return values
 phi minimum distance between each pair of bodies Jphi the kinematic jacobian of phi
 function collidingPoints ( obj , kinsol , points , collision_threshold )

idx = collidingPoints(obj, kinsol, points, collision_threshold) returns the indices of the points in collision with any element in the manipulator

Parameters
 obj kinsol result of calling doKinematics(obj, q) where q is a position vector. Can also be q, and we'll call doKinematics for you. points (3 x p) 3D coordinates of p points to check collision_threshold min distance allowed to consider the point not in collision
Return values
 indices of the points in collision with any element in the manipulator
 function collidingPointsCheckOnly ( obj , kinsol , points , collision_threshold )

is_in_collision = collidingPoints(obj, kinsol, points, collision_threshold) returns true if any point in points is in collision with the manipulator

Parameters
 obj kinsol result of calling doKinematics(obj, q) where q is a position vector. Can also be q, and we'll call doKinematics for you. points (3 x p) 3D coordinates of p points to check collision_threshold min distance allowed to consider the point not in collision
Return values
 bollean indicating whether any point in points is in collision with the manipulator
 function collisionDetect ( obj , kinsol , allow_multiple_contacts , active_collision_options )

[distance,normal,xA,xB,idxA,idxB] = collisionDetect(obj,kinsol) returns the points of closest approach between all pairs of collision elements in the manipulator, with the following exceptions:

• any body and it's % parent in the kinematic tree
• body A and body B, where body A belongs to collision filter groups that ignore all of the collision filter groups to which body B belongs as well as the points of closest approach between the manipulator's terrain contact points and terrain (if applicable). For a description of terrain contact points, see RigidBodyGeometry/getTerrainContactPoints

[] = collisionDetect(obj,kinsol,active_collision_options) returns % the same information as above, but only for those contact pairs that satisfy the criteria in active_collision_options (See below).

Parameters
 obj kinsol result of calling doKinematics(obj, q) where q is a position vector. Can also be q, and we'll call doKinematics for you. allow_multiple_contacts - Logical indicating whether or not the collision detection algorithm return multiple contact points for a single pair of contact elements. Default: false active_collision_options - Struct that may the following fields body_idx - vector of body indices. Only these bodies will be considered for collsion detection Default: - Consider all bodies collision_groups - cell array of strings. Only the collision geometry belonging to these groups will be considered for collision detection.Note that the filtering based on collision_filter_groups and adjacency in the kinematic tree still apply. Default: - Consider all groups terrain_only - Logical scalar. If true, only consider the interaction between the manipulator's terrain contact points and terrain. Default: - false
Return values
 phi - (m x 1) Vector of gap function values (typically contact distance), for m possible contacts normal - (3 x m) Contact normal vector in world coordinates, points from B to A xA - (3 x m) The closest point on body A to contact with body B, relative to body A origin and in body A frame xB - (3 x m) The closest point on body B to contact with body A, relative to body B origin and in body B frame idxA - (m x 1) The index of body A. idxB - (m x 1) The index of body B.
 function collisionDetectFromPoints ( obj , kinsol , points , use_margins )

Given a set of points in global frame, returns the signed distance of each point to the closest surface of the RBM.

Utilizes the same collision-checking machinery of collisionDetect(), but instead of checking distances between objects in the model, computes distances for the supplied points.

Parameters
 kinsol the output of doKinematics. Note that this method requires a kinsol with mex enabled points 3xN pointcloud in global frame use_margins whether the collision will be checked against artificially-inflated objects (which have better gradients)
Return values
 phi Nx1 signed distances n 3xN normals at nearest body x 3xN points on clearest body body_x 3xN points on clearest body body_idx Nx1 body idx of nearest to each point
 function collisionDetectTerrain ( obj , xA_in_world )
Return values
 phi normal xB idxB
 function collisionRaycast ( obj , kinsol , origins , ray_endpoints , use_margins )

function distance = collisionRaycast(obj,kinsol, origin, point_on_ray)

Uses bullet to perform a raycast, returning the distance or -1 on no hit.

See systems/plants/test/bullet_raycast_test.m for example usage.

Parameters
 kinsol result of calling doKinematics(obj, q) where q is a position vector. origin vector of size indicating the origin of the ray. Size is 3 x N. ray_endpoint vector of size 3 indicating the end point of the ray, specifying the direction and maximum length of the raycast. Size is 3 x N. use_margins boolean indicating whether or not to use a collision model whose boxes and meshes are padded with a margin to improve numerical stability of contact gradient. Default true.
Return values
 distance distance to the nearest hit on the ray, or -1 on no colliion. normal normal vector at collision surface, or [0;0;0] if no collision.
 function compile ( model )

After parsing, compute some relevant data structures that will be accessed in the dynamics and visualization.

Return values
 model
 function compositeRigidBodyInertias ( manipulator , inertias_world , dinertias_world )

Computes composite rigid body inertias expressed in world frame.

Parameters
 inertias_world cell array of 6x6 body inertia matrices, transformed to world frame dinertias_world gradient of inertias_world
Return values
 crbs cell array of composite rigid body inertias dcrbs gradient of crbs
 function computeFrictionForce ( model , v )

Note: gradient is with respect to v, not q!

Return values
 f_friction df_frictiondv
 function constructCOMFrame ( model )
Return values
 fr
 function constructInputFrame ( model )
protected

inputparents is an array of the parent RigidBodies of each joint.

Return values
 fr
 function constructParamFrame ( model )
protected
Return values
 model fr pval pmin pmax
 function constructStateFrame ( model )
protected
Return values
 model
 function constructVisualizer ( obj , options )
Return values
 v
 function contactConstraintDerivatives ( obj , normal , kinsol , idxA , idxB , xA , xB , d )
Return values
 n D dn dD
 function contactConstraints ( obj , kinsol , allow_multiple_contacts , active_collision_options )

function [phi,normal,d,xA,xB,idxA,idxB,mu,n,D,dn,dD] = contactConstraints(obj,kinsol,body_idx) Compute the contact constraints for a manipulator, and relevent bases.

The contact frame always points from body B to body A.

Parameters
 obj kinsol allow_multiple_contacts Allow multiple contacts per body pair. Optional, defaults to false. active_collision_options A optional struct to determine which bodies and collision groups are checked. See collisionDetect.
Return values
 phi (m x 1) Vector of gap function values (typically contact distance), for m possible contacts normal (3 x m) Contact normal vector in world coordinates, points from B to A d {k} (3 x m) Contact friction basis vectors in world coordinates, points from B to A xA (3 x m) The closest point on body A to contact with body B, relative to body A origin and in body A frame xB (3 x m) The closest point on body B to contact with body A, relative to body B origin and in body B frame idxA (m x 1) The index of body A. 0 is the special case for the environment/terrain idxB (m x 1) The index of body B. 0 is the special case for the environment/terrain mu (m x 1) Coefficients of friction n (m x n) normal vector in joint coordinates, state vector length n D {2k}(m x n) friction cone basis in joint coordinates, for k directions dn (mn x n) dn/dq derivative dD {2k}(mn x n) dD/dq derivative
 function contactConstraintsBV ( obj , varargin )

function [phi,B,JB,mu] = contactConstraintsBV(obj,kinsol,allow_multiple_contacts,active_collision_options) a variation on the contactConstraints function that uses a different representation of the friction cone–basis vectors are the extreme rays of the cone (no basis for the normal force)

Return values
 phi (m x 1) Vector of gap function values (typically contact distance), for m possible contacts B {m}(3 x 2k) friction polyhedron basis vectors JB {m}{2k x n} parameterization of the polyhedral approximation of the friction cone, in joint coordinates JB{k}(i,:) is the ith direction vector for the kth contact (of nC) mu (m x 1) Coefficients of friction normal (3 x m) Contact normal vector in world coordinates, points from B to A
Parameters
 See contactConstraints for argument list description
 function createMexPointer ( obj )
protected
Return values
 obj
 function doKinematics ( model , q , v , options , qd_old )

kinsol=doKinematics(model, q, v, options, qd_old) Precomputes information necessary to compute kinematic and dynamic quantities such as Jacobians (

forwardKin,
geometricJacobian), mass matrix and dynamics bias (
manipulatorDynamics), and other biases, e.g. Jdot * v (
geometricJacobianDotTimesV).
Parameters
 q joint position vector. Must be model.getNumPositions() x 1 v joint velocity vector. Must be model.getNumVelocities() x 1 or empty options options struct. Fields: use_mex: whether or not to use the mex implementation. Default: true compute_gradients: boolean, if true: additionally precompute gradients of all quantities in doKinematics that are precomputed when compute_gradients is false. Set options.compute_gradients to true if gradients of Jacobians or dynamics are required. If only Jacobians (geometric or from forwardKin) are required, setting compute_gradients to true is not required. Default: false compute_JdotV: whether or not to precompute quantities necessary to compute biases such as Jdot * v and the C term in manipulatorDynamics Default: true if v is passed in and not empty. kinematics_cache_ptr_to_use: DrakeMexPointer. Tells doKinematics to use a specific kinematics cache DrakeMexPointer. This option can be useful when multiple kinsols need to be valid at the same time. By default, the default kinematics cache pointers stored in the RigidBodyManipulator are used, meaning that subsequent doKinematics calls overwrite the previous cache, and hence invalidate kinsols from earlier doKinematics calls.
Return values
 kinsol a structure containing the precomputed information (non-mex case) or a certificate of having precomputed this information (mex case)

Note: the contents of kinsol may change with implementation details - do not attempt to use kinsol directly (our contract is simply that it can always be used by other kinematics and dynamics methods in Drake.

Note: old method signature: kinsol = doKinematics(model,q,b_compute_second_derivatives,use_mex,qd) This method signature is deprecated, please transition to using the options struct. Using the old signature is supported for now.

 function drawKinematicTree ( model )

depends on having graphviz installed todo: make that a dependency in configure?

 function drawLCMGLAxes ( model , lcmgl , q , body_indices )
 function drawLCMGLClosestPoints ( model , lcmgl , kinsol , varargin )
 function drawLCMGLForces ( model , q , qd , gravity_visual_magnitude )

draws the forces and torques on the robot.

They are spatial vectors (wrenches) and are drawn at the body's origin on which they act. Forces are in green and torques in purple. The magnitude of each vector is scaled by the same amount that the vector corresponding to the gravitational force on the robot would be scaled in order to make it have a norm equal to gravity_visual_magnitude. Use drawLCMGLGravity to draw the gravity.

Parameters
 q the position of the robot qd the velocities of the robot gravity_visual_magnitude specifies the (would-be) visual length of the vector representing the gravitational force.
 function drawLCMGLGravity ( model , q , gravity_visual_magnitude )

draws a vector centered at the robot's center of mass having the direction of the gravitational force on the robot.

Parameters
 q the position of the robot gravity_visual_magnitude specifies the visual length of the vector representing the gravitational force.
 function energy ( model , x )
Parameters
 x the state vector
Return values
 T the total kinetic energy U the total potential energy todo: add support for an optional robotnum argument?
 function extractFrameInfo ( obj , body_or_frame_index )
Return values
 body_index Tframe
 function findAncestorBodies ( obj , body_index )

findAncestorBodies Find indices of ancestors of a body

Parameters
 body_index index of a rigid body in this RigidBodyManipulator
Return values
 the indices of the parents of the rigid body with index body_index, not including body_index, ordered from closest to most distant ancestor
 function findFixedPoint ( obj , x0 , u0 , options )
Return values
 xstar ustar success
 function findFrameId ( model , name , robotnum )
Parameters
 name is the string name to search for robotnum if specified restricts the search to a particular robot
Return values
 frame_id
 function findJoint ( model , jointname , robot )
Return values
 body
 function findJointId ( model , jointname , robot_num , error_level )
Parameters
 robot_num can be the robot number or the name of a robot robot_num<0 means look at all robots
Return values
 body_id
 function findJointInd ( model , varargin )
Return values
 body_ind
 function findJointIndices ( model , str )
Return values
 indices
 function findKinematicPath ( obj , start_body , end_body , use_mex )

findKinematicPath computes the shortest path from one robot link to another

Parameters
 start_body index of rigid body at which the path should start end_body index of rigid body at which the path should end
Returns
body_path column vector of rigid body indices that represent the shortest path starting at start_body and ending at end_body, including start_body and end_body
joint_path indices of joints on shortest path from start_body to end_body
signs vector of traversal directions along joint path. +1 if joint is traversed from child to parent, -1 if joint is traversed from parent to child Throws an error if there is no path between the bodies
Return values
 body_path joint_path signs
Return values
 body
Parameters
 robot can be the robot number or the name of a robot robot<0 means look at all robots error_level >0 for throw error, 0 for throw warning, <0 for do nothing. Default: throw error
Return values
 body_id
 function findLinkInd ( model , varargin )
Return values
 body_ind
 function findPositionIndices ( model , str )

findPositionIndices Returns position indices in the state vector for joints whose name contains a specified string.

Parameters
 str (sub)string to be searched for indices array of indices into state vector
 function findVelocityIndices ( model , str )

findJointVelocityIndices Returns velocity indices in the state vector for joints whose name contains a specified string.

Parameters
 str (sub)string to be searched for indices array of indices into state vector
 function forwardJacDotTimesV ( obj , kinsol , body_or_frame_id , points , rotation_type , base_or_frame_id )

Computes Jd * v, where J is a matrix that maps from the joint velocity vector v to the time derivative of x (x and J of format specified in forwardKinV).

Gradient output is also available. Note that this is in general not the same as dx/dq * qd when the mapping from v to qd is time-variant!

Parameters
 body_or_frame_ind,an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd) points a 3 x m matrix where each column represents a point in the frame specified by body_or_frame_ind rotation_type integer flag indicated whether rotations and derivatives should be computed (0 - no rotations, 1 - rpy, 2 - quat) base_or_frame_ind an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd) Default: 1 (world)
forwardKin
Return values
 Jdot_times_v dJdot_times_v
 function forwardKin ( obj , kinsol , body_or_frame_id , points , options )

Transforms points given in a frame identified by body_or_frame_id to a frame identified by options.base_or_frame_id, and also computes a representation of the relative rotation (of type specified by rotation_type), stacked in a matrix x.

Also returns the Jacobian J that maps the joint velocity vector v to xdot, as well as the gradient of J with respect to the joint configuration vector q.

Parameters
 kinsol solution structure obtained from doKinematics body_or_frame_id,an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd) points a 3 x m matrix where each column represents a point in the frame specified by body_or_frame_id rotation_type integer flag indicating the type of rotation output to be included in the return values – 0, no rotation included – 1, output Euler angle – 2, output quaternion options.base_or_frame_id an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd) Default: 1 (world). in_terms_of_qdot boolean specifying whether to return the mapping from qdot to xdot (i.e. the gradient dx/dq) or v to xdot.
Return values
 x a matrix with m columns, such that column i is [points_base_i; q_rot], where points_base_i is points(:, i) transformed to the frame identified by body_or_frame_id, and q_rot is a representation of the relative rotation (the same for all m columns). That is, if rotation_type = 0 then x will be a 3xm matrix with column i equal to [points_base_i]. If rotation_type = 1 x will be 6xm, with column i equal to [points_base_i; rpy], and if rotation_type = 2, will be 7xm with column i equal to [points_base_i; quat] J the Jacobian that maps the joint velocity vector v or the derivative of the configuration vector, qd (depending on options.in_terms_of_qdot), to xd, the time derivative of x. dJ the gradient of J with respect to the coordinate vector q.

NOTE: old signature: forwardKin(obj, kinsol, body_or_frame_id, points, rotation_type)

 function geometricJacobian ( obj , kinsol , base , end_effector , expressed_in , in_terms_of_qdot )

GEOMETRICJACOBIAN Computes the geometric Jacobian from base to end_effector expressed in frame expressed_in.

Parameters
 kinsol solution structure obtained from doKinematics base index of base' rigid body end_effector index of end effector' rigid body expressed_in an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkId or findFrameInd) signifying in which frame the geometric Jacobian should be expressed in_terms_of_qdot boolean specifying whether to return the mapping from qdot to twist or v to twist
Return values
 J the geometric Jacobian such that the twist of end_effector with respect to base, expressed in expressed_in is J * (v(v_or_qdot_indices)) or J * (qdot(v_or_qdot_indices)) if in_terms_of_qdot v_or_qdot_indices vector of indices into the robot's joint velocity vector v. v_or_qdot_indices is guaranteed to be in order' in the following two ways: 1) if the joint associated with body' is on the path between base and endEffector, then the indices in body.velocity_num or body.position_num will appear in the same order in v_or_qdot_indices 2) if body1 precedes body2 in the joint path going from base to end_effector, then body1.velocity_num/position_num will precede body2.velocity_num/position_num in v_or_qdot_indices. Note that this guarantees that the last length(end_effector.velocity_num)/length(end_effector.position_num) indices of v_or_qdot_indices are exactly body.velocity_num/body.position_num. dJdq gradient of J with respect to joint configuration vector q
 function geometricJacobianDotTimesV ( obj , kinsol , base , end_effector , expressed_in )

geometricJacobianDotTimesV computes the convective term' d/dt(J) * v, where J is a geometric Jacobian and v is the vector of joint velocities across the joints on the path from base to end_effector.

Parameters
 kinsol solution structure obtained from doKinematics base base frame of the geometric Jacobian end_effector end effector frame of the geometric Jacobian expressed_in frame in which the geometric Jacobian is expressed
Return values
 d/dt(J) * v
 function geometricJacobianDotV ( obj , kinsol , base , endEffector , expressedIn )

GEOMETRICJACOBIANDOTV computes the 'convective term' d/dt(J) * v, where J is a geometric Jacobian and v is the vector of joint velocities across the same joints that the geometric Jacobian spans.

Parameters
 kinsol solution structure obtained from doKinematics twists twists of all links with respect to base, expressed in body frame base base frame of the geometric Jacobian endEffector end effector frame of the geometric Jacobian expressedIn frame in which the geometric Jacobian is expressed
Return values
 d/dt(J) * v
 function getActuatedJoints ( model )
Return values
 index
 function getB ( obj , q , qd )

Note: getB(obj) is ok iff there are no direct feedthrough force elements.

Return values
 B
 function getBody ( model , body_ind )
Return values
 body
 function getBodyOrFrameName ( model , body_or_frame_id )
Return values
 str
 function getCMM ( varargin )
 function getCMMdA ( varargin )
 function getCollisionGeometryGroupNames ( model )
Return values
 groups
 function getCOM ( model , kinsol , robotnum , in_terms_of_qdot )
Parameters
 robotnum – An int array. Default is 1 Not given, the COM of the whole model is computed. Otherwise, the bodies that belong to robot(robotnum) is computed note: for Jdot, call forwardJacDot with body_ind = 0
Return values
 com J dJ
 function getContactShapeGroupNames ( varargin )
Return values
 varargout
 function getFrame ( model , frame_id )
Return values
 frame
 function getGravity ( obj , grav )
Return values
 g
 function getInitialState ( obj )
virtual
Return values
 x0

Reimplemented from DrakeSystem.

 function getLinkName ( obj , body_ind )

str = getLinkName(obj,body_ind) returns a string containing the link name the specified body if body_ind is a scalar.

If body ind is a vector, it returns a cell array containing the names of the bodies specified by the elements of body_ind.

Parameters
 obj - RigidBodyManipulator object body_ind - Body index or vector of body indices
Return values
 str - String (cell array of strings) containing the requested linkname(s)
 function getMass ( model , robotnum )

todo: write total_mass to class and simply return it instead of looping every time (since the result is a constant between compiles)

Return values
 m
 function getMexModelPtr ( obj )

This access should not be allowed (the method should be private or not exist). But it is currently used by QPControlBlock to pass to QPControllermex in drc/software/control

Return values
 ptr
 function getNumBodies ( obj )
Return values
 n
 function getNumContactPairs ( obj )
Return values
 n
 function getNumUnilateralConstraints ( obj )

Returns the number of unilateral constraints, currently only contains the contact pairs.

Return values
 n
 function getParams ( model )
Return values
 p
 function getPositionFrame ( obj , robotnum )

if robotnum is not specified, then it returns a position frame including all position variables (for all robots)

Return values
 fr
 function getRandomConfiguration ( obj )
Return values
 q0
 function getStateFrame ( obj , robotnum )

if robotnum is not specified, then it returns a state frame including all state variables (for all robots)

Return values
 fr
 function getTerrainContactPoints ( obj , body_idx , contact_groups )

terrain_contact_point_struct = getTerrainContactPoints(obj) returns a structure array containing the terrain contact points on all bodies of this manipulator.

terrain_contact_point_struct = getTerrainContactPoints(obj,body_idx) returns a structure array containing the terrain contact points on the bodies specified by body_idx.

For a general description of terrain contact points see RigidBodyGeometry/getTerrainContactPoints

Parameters
 obj - RigidBodyManipulator object body_idx - vector of body-indices indicating the bodies for which terrain contact points should be found Default: All bodies except the world contact_groups - (optional) cell array of cell arrays containing contact group names for each body
Return values
 terrain_contact_point_struct - nx1 structure array, where n is the number of bodies with terrain contact points. Each element has the following fields idx - Index of a body in the RigidBodyManipulator pts - 3xm array containing points on the body specified by idx (in body frame) that can collide with arbitrary terrain.

 function getTerrainHeight ( obj , contact_pos )

Access method for querying the terrain height from the RigidBodyTerrain object assigned to this manipulator.

Parameters
 contact_pos a 2xN or 3xN list of positions in the world frame below which to compute the height of the terrain.
Return values
 z a 1xN list of terrain heights in world coordinates normal a 3xN list of surface normals in world cooridinates
 function getVelocityFrame ( obj , robotnum )

if robotnum is not specified, then it returns a velocity frame including all velocity variables (for all robots)

Return values
 fr
 function getZeroConfiguration ( obj )
Return values
 q
 function inertiasInWorldFrame ( manipulator , kinsol )

Transforms 6x6 inertia matrices from body frame to world frame.

Parameters
 kinsol solution structure from doKinematics
Return values
 inertias inertia matrices in world frame dinertias gradients of inertia matrices with respect to joint configuration vector q
 function inverseKin ( obj , q_seed , q_nom , varargin )

inverseKin(obj,q_seed,q_nom,constraint1,constraint2,constraint3,,options) % solve IK min_q (q-q_nom)'Q(q-q_nom) subject to constraint1 at t constraint2 at t

. % using q_seed as the initial guess

Parameters
 q_seed The seed guess, a column vector q_nom The nominal posture, a column vector constraint A RigidBodyConstraint class object, currently support SingleTimeKinematicConstraint, PostureConstraint and QuasiStaticConstraint. There can be at most 1 QuasiStaticConstraint ikoptions An IKoptions object, please refer to IKoptions for detail
Return values
 q the IK solution posture info 1,2,3 or 4 are acceptable 1, Successful, q is optimal and feasible 2, q is feasible 3, Solve the problem, but not to the desired accuracy 4, SNOPT thinks it is not feasible, but we check the constraint, and they are satisfied within the tolerance 13, the problem is infeasible. Set the debug equalt to true in the IKoptions by calling setDebug. And check infeasible_constraint to retrieve the name of the infeasible constraint 32, the problem terminates too early, change the major iterations limit in options by calling setMajorIterationsLimit function For any other info, it means the IK program is problematic, please contact the author infeasible_constraint Optional return argument. A cell of constraint names that are infeasible.
 function inverseKinBackend ( obj , mode , t , q_seed , q_nom , varargin )

inverseKin(obj,mode,q_seed,q_nom,t,constraint1,constraint2,,options) % Two modes, mode 1 – solve IK min_q (q-q_nom'Q(q-q_nom) subject to constraint1 at t constraint2 at t

. % using q_seed as the initial guess If t = [], then all kc are ative. If t is a scalar, then solve IK for that single time only If t is an array, then for each time, solve an IK

mode 2 – solve IK for a sequence of time min_qi sum_i qdd_i'Qa*qdd_i+qd_i'*Qv*qd_i+(q_i-q_nom(:,i))'*Q(q_i-_nom(:,i)) subject to constraint_j at t_i qd,qdd are computed by supposing q is a cubic spline.

Parameters
 q_seed The seed guess, a matrix, wich column i representing the seed at t[i] q_nom The nominal posture, a matrix, with column i representing The nominal posture at t[i] constraint A Constraint class object, accept SingleTimeKinematicConstraint, MultipleTimeKinematicConstraint, QuasiStaticConstraint, PostureConstraint and MultipleTimeLinearPostureConstraint currently ikoptions An IKoptions object, please refer to IKoptions for detail
Return values
 varargout
 function inverseKinPointwise ( obj , t , q_seed , q_nom , varargin )

inverseKin(obj,t,q_seed,q_nom,constraint1,constraint2,constraint3,,options) % solve IK at each sample time t(i) seperately min_q(:,i) (q(:,i)-q_nom(:,i))'Q(q(:,i)-q_nom(:,i)) subject to constraint1 at t(i) constraint22 at t(i)

. % using q_seed as the initial guess

Parameters
 t The sample time, a row vector q_seed The seed guess, a column vector q_nom The nominal posture, a column vector constraint A RigidBodyConstraint object, an object of SingleTimeKinematicConstraint class, A QuasiStaticConstraint, or a PostureConstraint, ikoptions an IKoptions object, please refer to IKoptions for detail
Return values
 q the IK solution posture info 1,2,3 or 4 are acceptable 1, Successful, q is optimal and feasible 2, q is feasible 3, Solve the problem, but not to the desired accuracy 4, SNOPT thinks it is not feasible, but we check the constraint, and they are satisfied within the tolerance 13, the problem is infeasible. Set the debug equalt to true in the IKoptions by calling setDebug. And check infeasible_constraint to retrieve the name of the infeasible constraint 32, the problem terminates too early, change the major iterations limit in options by calling setMajorIterationsLimit function For any other info, it means the IK program is problematic, please contact the author infeasible_constraint Optional return argument. A cell of constraint names that are infeasible.
 function inverseKinTraj ( obj , t , q_seed_traj , q_nom_traj , varargin )

inverseKinSequence(obj,t,q_seed_traj,q_nom_traj,constraint1,constraint2,constraint3,,options) % If options.fixInitialState = true solve IK min_q sum_i qdd(:,i)'Qa*qdd(:,i)+qd(:,i)'*Qv*qd(:,i)+(q(:,i)-q_nom(:,i))'*Q(q(:,i)-q_nom(:,i))] subject to constraint1 at t_samples(i) constraint2 at t_samples(i) % constraint(k) at [t_samples(2) t_samples(3) t_samples(nT)] % constraint(k+1) at [t_samples(2) t_samples(3) t_samples(nT)] % .

% using q_seed_traj as the initial guess. q(1) would be fixed to q_seed_traj.eval(t(1)) t_samples = unique([t,options.additional_tSamples]); In the objective function, the 'q,qd,qdd' are evaluated at knot time t only. If options.fixInitialState = false solve the same optimization problem, but q(1) and qdot(1) would be decision variables, and the constraints would be enforced at time t(1) also Note Qv is default to be zero. q is supposed to be a cubic spline, qdd and qd are the acceleration and velocity of this cubic spline.

Parameters
 t the knot time, a row vector q_seed_traj the seed guess, a Trajectory object q_nom_traj the nominal posture, a Trajectory object constraint A RigidBodyConstraint object, can be a SingleTimeKinematicConstraint object, A MultipleTimeKinematicConstraint, A QuasiStaticConstraint, or a PostureConstraint object options an IKoptions object
Return values
 xtraj the IK trajectory for x = [q;qdot]. A cubic spline. info 1,2,3 or 4 are acceptable 1, Successful, q is optimal and feasible 2, q is feasible 3, Solve the problem, but not to the desired accuracy 4, SNOPT thinks it is not feasible, but we check the constraint, and they are satisfied within the tolerance 13, the problem is infeasible. Set the debug equalt to true in the IKoptions by calling setDebug. And check infeasible_constraint to retrieve the name of the infeasible constraint 32, the problem terminates too early, change the major iterations limit in options by calling setMajorIterationsLimit function For any other info, it means the IK program is problematic, please contact the author infeasible_constraint Optional return argument. A cell of constraint names that are infeasible
 function inverseKinWrapup ( obj , q0 , varargin )

[q_seed,q_nom,constraint,ikoptions] = inverseKinWrapup(obj,q0, body1,body1_pts,body1_pos, % body1,body2_pts,body2_pos, % body1,body3_pts,body3_pos, % % bodyN,bodyN_pts,bodyN_pos, % options use_mex_constraint) The input to the function is the same input list used in the deprecated IK call, except the last one, which determines whether to construct the constraint in mex only or not.

Parameters
 obj The robot q0 The seed guess body The body index body_pts Points on the body body_pos The position of the body_pts, can also include gaze constraint options A struct we used before in the IK call use_mex_constraint A boolean flag, set to true, then all the kinematic constraint would be a mex object. This input is optional

This wraps up the old inverseKin interface to the new interface

Return values
 q_seed q_nom constraint ikoptions
 function isBodyPartOfRobot ( model , body , robotnum )

determines whether a given body is part of a robot specified by robotnum.

Parameters
 body a RigidBody robotnum vector of integers corresponding to the RigidBody.robotnum field.
Return values
 ret whether or not the body is part of a robot.
 function isValidLinkId ( obj , id )
Return values
 is_valid
 function isValidLinkIndex ( obj , idx )
Return values
 is_valid
 function leastCommonAncestor ( model , body1 , body2 )

recursively searches for the lowest body in the tree that is an ancestor to both body1 and body2

Return values
 b
 static function loadobj ( obj )
static
Return values
 obj
 function newBody ( model )
Return values
 body
 function output ( obj , t , x , u )
virtual

The outputs of this dynamical system are concatenated from each of the sensors.

If no sensor has been added to the system, then the output defaults to the full robot state.

Return values
 y

Reimplemented from Manipulator.

 function pairwiseClosestDistance ( obj , kinsol , bodyA_idx , bodyB_idx )

Uses bullet to find the minimum distance between bodies A and B.

Parameters
 kinsol the output from doKinematics() bodyA_idx numerical index of rigid body A or (less efficient:) a rigidbody object bodyB_idx numerical index of rigid body B (or the rigidbody object)
Return values
 phi minimum distance between bodies A and B Jphi the kinematic jacobian of phi
 function pairwiseClosestPoints ( obj , kinsol , bodyA_idx , bodyB_idx )

Uses bullet to find the points of closest approach between bodies A and B.

Parameters
 kinsol the output from doKinematics() bodyA_idx numerical index of rigid body A or (less efficient:) a rigidbody object bodyB_idx numerical index of rigid body B (or the rigidbody object) if bodyB_idx is -1, compute collisions with entire world
Return values
 ptA the points (in world coordinates) on bodyA that are in collision ptB the point (in world coordinates) on bodyB that are in collision normal the contact normal vectors on body B in world coordinates JA the jacobian of ptA JB the jacobian of ptB
 function pairwiseContactConstraints ( obj , kinsol , body_indA , body_indB , body_collision_indA )

Uses bullet to perform collision detection between rigid body A and rigid body B, and returns a single penetration constraint.

Parameters
 kinsol the output from doKinematics() body_indA numerical index of rigid body A or (less efficient:) a rigidbody object body_indB numerical index of rigid body B (or the rigidbody object) if body_indB is -1, compute collisions with entire world body_collision_indA index vector of collision objects on body A
Return values
 phi phi(i,1) is the signed distance from the contact point on body A to the contact on body B the robot to the closes object in the world. phi is set to 1 if not in contact n the surface "normal vector", but in joint coordinates (eq 3 in Anitescu97) n(i,:) is the normal for the ith contact D parameterization of the polyhedral approximation of the friction cone, in joint coordinates (figure 1 from Stewart96) D{k}(i,:) is the kth direction vector for the ith contact (of nC) mu mu(i,1) is the coefficient of friction for the ith contact
 function pairwiseContactConstraintsBV ( obj , kinsol , body_indA , body_indB , body_collision_indA )

Uses bullet to perform collision detection between rigid body A and rigid body B, and returns a single penetration constraint.

returns signed contact distance and a slightly different representation of the friction cone approx–basis vectors along the edges of the polyhedron, positive combinations of which lie within the friction cone.

Parameters
 kinsol the output from doKinematics() body_indA numerical index of rigid body A or (less efficient:) a rigidbody object body_indB numerical index of rigid body B (or the rigidbody object) if body_indB is -1, compute collisions with entire world body_collision_indA index vector of collision objects on body A
Return values
 phi phi(i,1) is the signed distance from the contact point on body A to the contact on body B the robot to the closes object in the world. phi is set to 1 if not in contact B friction polyhedron basis vectors JB parameterization of the polyhedral approximation of the friction cone, in joint coordinates JB{k}(i,:) is the ith direction vector for the kth contact (of nC) mu mu(i,1) is the coefficient of friction for the ith contact
 function pairwiseContactDistance ( obj , kinsol , body_indA , body_indB )

Uses bullet to perform collision detection between rigid body A and rigid body B, and returns a single penetration constraint.

Parameters
 kinsol the output from doKinematics() body_indA numerical index of rigid body A or (less efficient:) a rigidbody object body_indB numerical index of rigid body B (or the rigidbody object)
Return values
 phi 1 if no contact; sum of the penetration depths if there is contact Jphi the kinematic jacobian of phi
 function pairwiseContactTest ( obj , kinsol , body_indA , body_indB , body_collision_indA )

Uses bullet to perform collision detection between rigid body A and rigid body B.

Parameters
 kinsol the output from doKinematics() body_indA numerical index of rigid body A or (less efficient:) a rigidbody object body_indB numerical index of rigid body B (or the rigidbody object) if body_indB is -1, compute collisions with entire world body_collision_indA index vector of collision objects on body A
Return values
 ptsA the points (in world coordinates) on bodyA that are in collision ptsB the point (in world coordinates) on bodyB that are in collision normal the contact normal vectors on body B in world coordinates JA the jacobian of ptsA JB the jacobian of ptsB
 function parseBodyOrFrameID ( obj , body_or_frame , robotnum )

body_idx = parseBodyOrFrameID(obj,body_or_frame) returns the body index or frame id associated with the input.

Parameters
 obj – RigidBodyManipulator object body_or_frame – Can be either: Numeric body index or frame id String containing body or frame name robotnum – Scalar restricting the search to a particular robot. Optional. Default: -1 (all robots)
Return values
 body_idx_or_frame_id – Numeric body index or frame id
 static function parseContactOptions ( options )
static

contact_options = parseContactOptions(options) returns a struct containing settings for contact/collision detection.

If no value is given for a particular setting, a default value is used.

Parameters
 options - Structure that may have fields specifying values for contact options.
Return values
 contact_options - Struct with the following fields: ignore_self_collisions Default: false replace_cylinders_with_capsules Default: true use_bullet Default: checkDependency('bullet') If a corresponding field exists in options, its value will be used.
 function parseMaterial ( node , options )
Return values
 c options
 function parseParamString ( model , robotnum , str )

Parses parameter strings from URDFs and returns either the value or a msspoly expression for later use with RigidBodyElement.bindParams().

Parameters
 model model we are building from a URDF robotnum robot number str string to parse
Return values
 val parameter value (possibly with msspolys inside) to put in the property of the object default_val value of the parameter when default values of params are used
 function pdcontrol ( sys , Kp , Kd , index )

creates new blocks to implement a PD controller, roughly illustrated by q_d —>[ Kp ]–>(+)–—>[ sys ]----——> yout | - | -——[ Kp,Kd ]<-—

when invoked with a single output argument: newsys = pdcontrol(sys,) % then it returns a new system which contains the new closed loop system containing the PD controller and the plant.

when invoked with two output arguments: [pdff,pdfb] = pdcontrol(sys,) % then it return the systems which define the feed-forward path and feedback-path of the PD controller (but not the closed loop system).

Parameters
 Kp a num_u x num_u matrix with the position gains Kd a num_u x num_u matrix with the velocity gains index a num_u dimensional vector specifying the mapping from q to u. index(i) = j indicates that u(i) actuates q(j). Default:: 1:num_u

For example, the a 2D floating base (with adds 3 passive joints in positions 1:3)model with four actuated joints in a serial chain might have Kp = diag([10,10,10,10]) Kd = diag([1, 1, 1, 1]) and the default index would automatically be index = 4:7

Return values
 varargout
 function processCFGroupArgs ( model , linknames , robotnums )
Parameters
 linknames Names of the links to be added to the collision filter group. Can be a string containing the name of a link, a cell-array containing the names of multiple links, or a vector of link indices robotnum Robot number of the links to be added to the collision filter group. Can be a vector or a 1-d cell-array.
Return values
 function qdotToV ( obj , kinsol )
Return values
 Vq dVq
 function relativeTransform ( obj , kinsol , base , body )

Computes the transform that maps vectors from body to base.

Parameters
 kinsol solution structure obtained from doKinematics base an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkId or findFrameInd) signifying the base frame. body an integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkId or findFrameInd) signifying the body frame.
Return values
 T transform from body frame to base frame dTdq gradient of T with respect to robot position vector q.
 function removeCollisionGroups ( model , contact_groups , robotnum )

model = removeCollisionGroups(model,contact_groups,robotnum) returns the model with the specified contact groups removed

Parameters
 model – RigidBodyManipulator object contact_groups – String or cell array of strings specifying the contact groups to be removed robotnum – Vector of robot indices to which operation will be restricted. Optional. Default: 1:numel(model.name)
Return values
 model
 function removeCollisionGroupsExcept ( model , contact_groups , robotnum , body_ids )

model = removeCollisionGroups(model,contact_groups,robotnum) returns the model with all contact groups removed except for those specified

Parameters
 model – RigidBodyManipulator object contact_groups – String or cell array of strings specifying the contact groups to be preserved. robotnum – Vector of robot indices to which operation will be restricted. Optional. Default: 1:numel(model.name) body_ids – Vector of body indices to which operation will be restricted. Optional. Default: 1:numel(model.body)
Return values
 model
 function removeFixedJoints ( model )
protected

takes any fixed joints out of the tree, adding their visuals and inertia to their parents, and connecting their children directly to their parents

Return values
 model
 function removeFromIgnoredListOfCollisionFilterGroup ( model , ignored_collision_fgs , collision_fg_name )

Removes the specified collision filter groups from the "ignored" list of another collision filter group.

Parameters
 ignored_collision_fgs Cell array of strings containing the names of the collision filter groups that should no longer be ignored collision_fg_name String giving the name of the collision filter group that should no longer ignore the groups specified by ignored_collision_fgs
Return values
 model

Removes links from the specified collision filter group.

Parameters
 linknames Names of the links to be removed from the collision filter group. Can be a string containing the name of a link, a cell-array containing the names of multiple links, or a vector of link indices collision_fg_name String containing the name of the collision filter group from which the links should be removed robotnums Robot number of the links to be removed from the collision filter group. Can be a vector or a 1-d cell-array. If non-scalar, it must have the same number of elements as linknames.
Return values
 model
 function removeShapeFromBody ( varargin )
Return values
 varargout
 function removeTerrainGeometries ( varargin )
Return values
 obj
 function removeTerrainGeometry ( obj )
Return values
 obj
 function removeVisualGeometryFromBody ( obj , body_id , geometry_name )

Removes all geometry from a given body that match a name.

Parameters
 body_id body to remove geometry from geometry_name name to match (will remove the first match)
Return values
 obj updated object
 function replaceCollisionGeometryWithConvexHull ( model , body_indices , varargin )
Return values
 model
 function replaceContactShapesWithCHull ( varargin )
Return values
 varargout
 function setBody ( model , body_ind , body )
Return values
 model
 function setForce ( model , force_ind , force )
Return values
 model
 function setFrame ( model , frame_id , frame )
Return values
 model
 function setGravity ( obj , grav )
Return values
 obj
 function setJointLimits ( obj , jl_min , jl_max )
Return values
 obj
 function setParams ( model , p )
Return values
 model
 function setTerrain ( obj , terrain )

Set the ground height profile which the contact points on the robot may not penetrate.

Parameters
 terrain a RigidBodyTerrain object
Return values
 obj
 function spatialAccelerations ( obj , kinsol , vd , root_accel )

Computes the spatial accelerations (time derivatives of twists) of all bodies in the RigidBodyManipulator, expressed in world.

Parameters
 kinsol solution structure obtained from doKinematics vd joint acceleration vector root_accel spatial acceleration of root body Default: zeros(6, 1)
Return values
 ret cell array containing spatial accelerations of all rigid bodies with respect to the world, expressed in world
 function surfaceTangents ( obj , normal )

% compute tangent vectors, according to the description in the last paragraph of Stewart96, p.2678

Return values
 d
 function terrainContactJacobianDotTimesV ( obj , kinsol , varargin )

Computes dJ/dt * v, where J is the matrix that maps the velocity vector v to a stacked vector of terrain contact point velocities.

Return values
 Jdot_times_v dJdot_times_v
 function terrainContactPositions ( obj , kinsol , varargin )

x_in_world = terrainContactPositions(obj,kinsol) returns the world-frame position of all terrain contact points on the manipulator.

[x_in_world,J] = terrainContactPositions(obj,kinsol) also returns the derivative of those positions w.r.t the manipulator's position variables.

[x_in_world,J,dJ] = terrainContactPositions(obj,kinsol) also returns the second derivative of those positions w.r.t the manipulator's position variables.

[] = terrainContactPositions(obj,kinsol,body_idx) % considers only the bodies indicated by body_idx.

[] = terrainContactPositions(obj,kinsol, % terrain_contact_point_struct) considers the body-frame points % specified by terrain_contact_point_struct. See RigidBodyManipulator/getTerrainContactPoints for a description of terrain_contact_point_struct.

[] = terrainContactPositions(obj,kinsol,,compute_Jdot_instead_of_dJ) % returns the time derivative of J rather than dJ when second % derivatives are requested.

For a general description of terrain contact points see RigidBodyGeometry/getTerrainContactPoints

Parameters
 obj - RigidBodyManipulator object kinsol - Structure returned by doKinematics body_idx - vector of body indices Default: =""> terrain_contact_point_struct - See description in RigidBodyManipulator/getTerrainContactPoints
Return values
 x_in_world - world-frame position of the terrain contact points J - dx_in_world/dq dJ - d^2x_in_world/dq^2
 function twists ( obj , transforms , q , v )
Return values
 ret
 function unilateralConstraints ( obj , x )
Return values
 phi dphi
 function vToqdot ( obj , kinsol )
Return values
 VqInv dVqInv
 function weldJoint ( model , body_ind_or_joint_name , robot )
Return values
 model

## Member Data Documentation

 Property actuator
protected

array of RigidBodyActuator objects

 Property B
 Property body
protected

array of RigidBody objects

 Property cached_terrain_contact_points_struct
protected

struct containing the output of 'obj.getTerrainContactPoints()'.

That output does not change between compilations, and is requested at every simulation dt, so storing it can speed things up.

 Property collision_filter_groups
 Property contact_constraint_id
protected
 Property contact_options
protected
protected

default kinematics caches; temporary way to make it easy to avoid creating and destroying DrakeMexPointers to KinematicsCache every time doKinematics is called

protected
 Property dim
protected
 Property dirty
 Property force
protected

cell array of RigidBodyForceElement objects

 Property frame
protected

array of RigidBodyFrame objects

 Property gravity
protected

note: need {} for arrays that will have multiple types (e.g.

a variety of derived classes), but can get away with [] for arrays with elements that are all exactly the same type

 Property loop
protected

array of RigidBodyLoop objects

 Property mex_model_ptr
 Property name
protected

names of the objects in the rigid body system

 Property num_contact_pairs
protected
 Property param_db
protected

cell of structures (one per robot) containing parsed parameter info

 Property quat_norm_constraint_id
protected

indices of state constraints asserting that the quaternion coordinates have norm=1

 Property robot_position_frames
protected
 Property robot_state_frames
protected
 Property robot_velocity_frames
protected
 Property sensor
protected

cell array of RigidBodySensor objects

 Property terrain
protected
 Property urdf
protected

names of the urdf files loaded

The documentation for this class was generated from the following file:
• /home/ubuntu/workspace/linux-gcc-continuous-matlab-documentation/drake/matlab/systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m