Drake
 All Classes Files Functions Variables
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 addTerrainGeometries (varargin)
 
function addTerrainGeometry (obj)
 
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)
 
function addSensor (model, sensor)
 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 findLinkInd (model, varargin)
 
function isValidLinkIndex (obj, idx)
 
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 addContactShapeToBody (varargin)
 
function addCollisionGeometryToBody (obj, body_id, geometry, varargin)
 obj = addCollisionGeometryToBody(obj,body_id,geometry,group_name) More...
 
function addVisualShapeToBody (varargin)
 
function addVisualGeometryToBody (obj, body_id, geometry)
 obj = addCollisionGeometryToBody(obj,body_id,geometry) More...
 
function addShapeToBody (varargin)
 
function addGeometryToBody (obj, body_id, geometry, varargin)
 obj = addGeometryToBody(obj,body_id,geometry) More...
 
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)
 
function addLinksToCollisionFilterGroup (model, linknames, collision_fg_name, robotnums)
 Adds links to the specified collision filter group. More...
 
function removeLinksFromCollisionFilterGroup (model, linknames, collision_fg_name, robotnums)
 Removes links from the specified collision filter group. More...
 
function addToIgnoredListOfCollisionFilterGroup (model, ignored_collision_fgs, collision_fg_name)
 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 processCFGroupArgs (model, linknames, robotnums)
 
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)
 
function changeRootLink (rbm, link, xyz, rpy, options)
 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...
 
function addPositionEqualityConstraint (obj, con, position_ind)
 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)
 
function addVelocityEqualityConstraint (obj, con, velocity_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...
 
function cascade (sys1, sys2)
 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 addStateConstraint (obj, con, xind)
 
function updateStateConstraint (obj, id, con, xind)
 
function displayStateConstraints (obj)
 
function removeAllStateConstraints (obj)
 
function addStateConstraintsToProgram (obj, prog, indices)
 adds state constraints and unilateral constraints to the program on the specified indices. More...
 
function addInputConstraintsToProgram (obj, prog, indices)
 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...
 
function cascade (sys1, sys2)
 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)
 deprecated method (due to refactoring): please use extractTrigPolySystem instead More...
 
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...
 
function cascade (sys1, sys2)
 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...
 
function addStateConstraintsToProgram (obj, prog, indices)
 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 setSimulinkParam (obj, varargin)
 Sets parameters of the simulink model Syntax setSimulinkParam(obj,param_name,param_value[,param_name,param_value,]) % Se. 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 loadobj (obj)
 
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...
 
function adjustContactShapes (varargin)
 
function adjustCollisionGeometry (model)
 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...
 
Property default_kinematics_cache_ptr_no_gradients
 default kinematics caches; temporary way to make it easy to avoid creating and destroying DrakeMexPointers to KinematicsCache every time doKinematics is called More...
 
Property default_kinematics_cache_ptr_with_gradients
 
Property frame
 array of RigidBodyFrame objects More...
 
Property robot_state_frames
 
Property robot_position_frames
 
Property robot_velocity_frames
 

Kinematic Tree

function getNumBodies (obj)
 
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. More...
 
function addLink (model, b)
 
function addJoint (model, name, type, parent_ind, child_ind, xyz, rpy, axis, damping, coulomb_friction, static_friction, coulomb_window, limits)
 
function addFloatingBase (model, parent, rootlink, xyz, rpy, joint_type)
 note that the case matters. More...
 
function findLinkId (model, linkname, robot, error_level)
 
function isValidLinkId (obj, id)
 
function getBody (model, body_ind)
 
function setBody (model, body_ind, body)
 
function addFrame (model, frame)
 
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 findLink (model, linkname, varargin)
 
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...
 
function addRobotFromURDFString (model, urdf_string, varargin)
 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 = addCollisionGeometryToBody(obj,body_id,geometry,group_name)

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
function addContactShapeToBody ( varargin  )
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   
)

obj = addGeometryToBody(obj,body_id,geometry)

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
function addLink ( model  ,
 
)
Return values
model
id
function addLinksToCollisionFilterGroup ( model  ,
linknames  ,
collision_fg_name  ,
robotnums   
)

Adds links to the specified collision filter group.

Parameters
linknamesNames 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
collision_fg_nameString containing the name of the collision filter group to which the links should be added
robotnumsRobot number of the links to be added to 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 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_filenamefilename 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_filenamefilename 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_stringxml 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
modelexisting RigidBodyManipulator the sensor should be added to
sensorsensor to add
Return values
newRigidBodyManipulator with the sensor added.
function addShapeToBody ( varargin  )
Return values
varargout
function addTerrainGeometries ( varargin  )
Return values
obj
function addTerrainGeometry ( 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_fgsCell array of strings containing the names of the collision filter groups to be ignored.
collision_fg_nameString 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   
)

obj = addCollisionGeometryToBody(obj,body_id,geometry)

Parameters
obj- RigidBodyManipulator object
body_id- Body index or body name
geometry- RigidBodyGeometry (or child class) object
Return values
obj
function addVisualShapeToBody ( varargin  )
Return values
varargout
function adjustCollisionGeometry ( model  )
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
function adjustContactShapes ( varargin  )
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
kinsolthe output of doKinematics. Note that this method requires a kinsol with mex enabled
Return values
ptsAthe i-th column contains the collision point (as identified by Bullet) on body A (in world coordinates)
ptsBthe i-th column contains the collision point (as identified by Bullet) on body B (in world coordinates)
idxAlink indices for the first body in each collision
idxBlink 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
qthe IK solution posture
info0 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
kinsolsolution structure obtained from doKinematics
body_or_frame_ind,aninteger ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkId or findFrameInd)
Return values
xthe position of pts (given in the global frame) in the body frame
Jthe Jacobian, dxdq
Pthe gradient, dxdpts - useful when computing forces
dJthe 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_indis an index of the body
pointis a point on the rigid body (in body coords)
forceis 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

See Also
centroidalMOmentumMatrixDotTimesV
Parameters
kinsolsolution structure obtained from doKinematics
robotnuman int array. Jdot * v for the bodies that belong to robot(robotnum) is computed. Default is 1.
Return values
com_Jacobian_dot_times_vJdot * v
dcom_Jacobian_dot_times_vgradient 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
kinsolsolution structure obtained from doKinematics
Return values
ACMM
dAgradient of A with respect to coordinate vector q
function centroidalMomentumMatrixDotTimesV ( obj  ,
kinsol  ,
robotnum   
)
Return values
Adot_times_v
dAdot_times_v
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
linkthe name or index of the link that will become the root link
xyzlocation on link (in the link frame) for the new origin
rpyorientation on link (in the link frame) for the new origin
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
kinsolthe 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
phiminimum distance between each pair of bodies
Jphithe 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
kinsolresult 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_thresholdmin distance allowed to consider the point not in collision
Return values
indicesof 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
kinsolresult 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_thresholdmin distance allowed to consider the point not in collision
Return values
bolleanindicating 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
kinsolresult 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
kinsolthe output of doKinematics. Note that this method requires a kinsol with mex enabled
points3xN pointcloud in global frame
use_marginswhether the collision will be checked against artificially-inflated objects (which have better gradients)
Return values
phiNx1 signed distances
n3xN normals at nearest body
x3xN points on clearest body
body_x3xN points on clearest body
body_idxNx1 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
kinsolresult of calling doKinematics(obj, q) where q is a position vector.
originvector of size indicating the origin of the ray. Size is 3 x N.
ray_endpointvector 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_marginsboolean 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
distancedistance to the nearest hit on the ray, or -1 on no colliion.
normalnormal 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_worldcell array of 6x6 body inertia matrices, transformed to world frame
dinertias_worldgradient of inertias_world
Return values
crbscell array of composite rigid body inertias
dcrbsgradient of crbs
function computeFrictionForce ( model  ,
 
)

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  ,
 
)
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_contactsAllow multiple contacts per body pair. Optional, defaults to false.
active_collision_optionsA 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
SeecontactConstraints for argument list description
function createMexPointer ( obj  )
protected
Return values
obj
function doKinematics ( model  ,
,
,
options  ,
qd_old   
)

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

See Also
forwardKin,
geometricJacobian), mass matrix and dynamics bias (
manipulatorDynamics), and other biases, e.g. Jdot * v (
geometricJacobianDotTimesV).
Parameters
qjoint position vector. Must be model.getNumPositions() x 1
vjoint velocity vector. Must be model.getNumVelocities() x 1 or empty
optionsoptions 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
kinsola 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  ,
,
body_indices   
)
function drawLCMGLClosestPoints ( model  ,
lcmgl  ,
kinsol  ,
varargin   
)
function drawLCMGLForces ( model  ,
,
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
qthe position of the robot
qdthe velocities of the robot
gravity_visual_magnitudespecifies the (would-be) visual length of the vector representing the gravitational force.
function drawLCMGLGravity ( model  ,
,
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
qthe position of the robot
gravity_visual_magnitudespecifies the visual length of the vector representing the gravitational force.
function energy ( model  ,
 
)
Parameters
xthe state vector
Return values
Tthe total kinetic energy
Uthe 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_indexindex of a rigid body in this RigidBodyManipulator
Return values
theindices 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
nameis the string name to search for
robotnumif 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_numcan 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_bodyindex of rigid body at which the path should start
end_bodyindex 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
function findLink ( model  ,
linkname  ,
varargin   
)
Return values
body
function findLinkId ( model  ,
linkname  ,
robot  ,
error_level   
)
Parameters
robotcan 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,aninteger ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd)
pointsa 3 x m matrix where each column represents a point in the frame specified by body_or_frame_ind
rotation_typeinteger flag indicated whether rotations and derivatives should be computed (0 - no rotations, 1 - rpy, 2 - quat)
base_or_frame_indan integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd)
Default: 1 (world)
See Also
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
kinsolsolution structure obtained from doKinematics
body_or_frame_id,aninteger ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd)
pointsa 3 x m matrix where each column represents a point in the frame specified by body_or_frame_id
rotation_typeinteger 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_idan integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkInd or findFrameInd)
Default: 1 (world).
in_terms_of_qdotboolean specifying whether to return the mapping from qdot to xdot (i.e. the gradient dx/dq) or v to xdot.
Return values
xa 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]
Jthe 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.
dJthe 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
kinsolsolution structure obtained from doKinematics
baseindex of `base' rigid body
end_effectorindex of `end effector' rigid body
expressed_inan 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_qdotboolean specifying whether to return the mapping from qdot to twist or v to twist
Return values
Jthe 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_indicesvector 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.
dJdqgradient 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
kinsolsolution structure obtained from doKinematics
basebase frame of the geometric Jacobian
end_effectorend effector frame of the geometric Jacobian
expressed_inframe 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
kinsolsolution structure obtained from doKinematics
twiststwists of all links with respect to base, expressed in body frame
basebase frame of the geometric Jacobian
endEffectorend effector frame of the geometric Jacobian
expressedInframe in which the geometric Jacobian is expressed
Return values
d/dt(J)* v
function getActuatedJoints ( model  )
Return values
index
function getB ( obj  ,
,
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  )

Please do note use this.

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.

See also RigidBodyGeometry/getTerrainContactPoints, RigidBodyManipulator/terrainContactPositions

function getTerrainHeight ( obj  ,
contact_pos   
)

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

Parameters
contact_posa 2xN or 3xN list of positions in the world frame below which to compute the height of the terrain.
Return values
za 1xN list of terrain heights in world coordinates
normala 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
kinsolsolution structure from doKinematics
Return values
inertiasinertia matrices in world frame
dinertiasgradients 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_seedThe seed guess, a column vector
q_nomThe nominal posture, a column vector
constraintA RigidBodyConstraint class object, currently support SingleTimeKinematicConstraint, PostureConstraint and QuasiStaticConstraint. There can be at most 1 QuasiStaticConstraint
ikoptionsAn IKoptions object, please refer to IKoptions for detail
Return values
qthe IK solution posture
info1,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_constraintOptional return argument. A cell of constraint names that are infeasible.
function inverseKinBackend ( obj  ,
mode  ,
,
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_seedThe seed guess, a matrix, wich column i representing the seed at t[i]
q_nomThe nominal posture, a matrix, with column i representing The nominal posture at t[i]
constraintA Constraint class object, accept SingleTimeKinematicConstraint, MultipleTimeKinematicConstraint, QuasiStaticConstraint, PostureConstraint and MultipleTimeLinearPostureConstraint currently
ikoptionsAn IKoptions object, please refer to IKoptions for detail
Return values
varargout
function inverseKinPointwise ( obj  ,
,
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
tThe sample time, a row vector
q_seedThe seed guess, a column vector
q_nomThe nominal posture, a column vector
constraintA RigidBodyConstraint object, an object of SingleTimeKinematicConstraint class, A QuasiStaticConstraint, or a PostureConstraint,
ikoptionsan IKoptions object, please refer to IKoptions for detail
Return values
qthe IK solution posture
info1,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_constraintOptional return argument. A cell of constraint names that are infeasible.
function inverseKinTraj ( obj  ,
,
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
tthe knot time, a row vector
q_seed_trajthe seed guess, a Trajectory object
q_nom_trajthe nominal posture, a Trajectory object
constraintA RigidBodyConstraint object, can be a SingleTimeKinematicConstraint object, A MultipleTimeKinematicConstraint, A QuasiStaticConstraint, or a PostureConstraint object
optionsan IKoptions object
Return values
xtrajthe IK trajectory for x = [q;qdot]. A cubic spline.
info1,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_constraintOptional 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
objThe robot
q0The seed guess
bodyThe body index
body_ptsPoints on the body
body_posThe position of the body_pts, can also include gaze constraint
optionsA struct we used before in the IK call
use_mex_constraintA 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
bodya RigidBody
robotnumvector of integers corresponding to the RigidBody.robotnum field.
Return values
retwhether 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  ,
,
,
 
)
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
kinsolthe output from doKinematics()
bodyA_idxnumerical index of rigid body A or (less efficient:) a rigidbody object
bodyB_idxnumerical index of rigid body B (or the rigidbody object)
Return values
phiminimum distance between bodies A and B
Jphithe 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
kinsolthe output from doKinematics()
bodyA_idxnumerical index of rigid body A or (less efficient:) a rigidbody object
bodyB_idxnumerical index of rigid body B (or the rigidbody object) if bodyB_idx is -1, compute collisions with entire world
Return values
ptAthe points (in world coordinates) on bodyA that are in collision
ptBthe point (in world coordinates) on bodyB that are in collision
normalthe contact normal vectors on body B in world coordinates
JAthe jacobian of ptA
JBthe 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
kinsolthe output from doKinematics()
body_indAnumerical index of rigid body A or (less efficient:) a rigidbody object
body_indBnumerical index of rigid body B (or the rigidbody object) if body_indB is -1, compute collisions with entire world
body_collision_indAindex vector of collision objects on body A
Return values
phiphi(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
nthe surface "normal vector", but in joint coordinates (eq 3 in Anitescu97) n(i,:) is the normal for the ith contact
Dparameterization 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)
mumu(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
kinsolthe output from doKinematics()
body_indAnumerical index of rigid body A or (less efficient:) a rigidbody object
body_indBnumerical index of rigid body B (or the rigidbody object) if body_indB is -1, compute collisions with entire world
body_collision_indAindex vector of collision objects on body A
Return values
phiphi(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
Bfriction polyhedron basis vectors
JBparameterization 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)
mumu(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
kinsolthe output from doKinematics()
body_indAnumerical index of rigid body A or (less efficient:) a rigidbody object
body_indBnumerical index of rigid body B (or the rigidbody object)
Return values
phi1 if no contact; sum of the penetration depths if there is contact
Jphithe 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
kinsolthe output from doKinematics()
body_indAnumerical index of rigid body A or (less efficient:) a rigidbody object
body_indBnumerical index of rigid body B (or the rigidbody object) if body_indB is -1, compute collisions with entire world
body_collision_indAindex vector of collision objects on body A
Return values
ptsAthe points (in world coordinates) on bodyA that are in collision
ptsBthe point (in world coordinates) on bodyB that are in collision
normalthe contact normal vectors on body B in world coordinates
JAthe jacobian of ptsA
JBthe 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
objRigidBodyManipulator 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
modelmodel we are building from a URDF
robotnumrobot number
strstring to parse
Return values
valparameter value (possibly with msspolys inside) to put in the property of the object
default_valvalue 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
Kpa num_u x num_u matrix with the position gains
Kda num_u x num_u matrix with the velocity gains
indexa 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
linknamesNames 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
robotnumRobot number of the links to be added to the collision filter group. Can be a vector or a 1-d cell-array.
Return values
linknames
robotnums
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
kinsolsolution structure obtained from doKinematics
basean integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkId or findFrameInd) signifying the base frame.
bodyan integer ID for a RigidBody or RigidBodyFrame (obtained via e.g., findLinkId or findFrameInd) signifying the body frame.
Return values
Ttransform from body frame to base frame
dTdqgradient 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
modelRigidBodyManipulator 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
modelRigidBodyManipulator 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_fgsCell array of strings containing the names of the collision filter groups that should no longer be ignored
collision_fg_nameString giving the name of the collision filter group that should no longer ignore the groups specified by ignored_collision_fgs
Return values
model
function removeLinksFromCollisionFilterGroup ( model  ,
linknames  ,
collision_fg_name  ,
robotnums   
)

Removes links from the specified collision filter group.

Parameters
linknamesNames 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_nameString containing the name of the collision filter group from which the links should be removed
robotnumsRobot 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_idbody to remove geometry from
geometry_namename to match (will remove the first match)
Return values
objupdated 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  ,
 
)
Return values
model
function setTerrain ( obj  ,
terrain   
)

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

Parameters
terraina 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
kinsolsolution structure obtained from doKinematics
vdjoint acceleration vector
root_accelspatial acceleration of root body
Default: zeros(6, 1)
Return values
retcell 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: <All bodies>="">
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  ,
,
 
)
Return values
ret
function unilateralConstraints ( obj  ,
 
)
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
Property default_kinematics_cache_ptr_no_gradients
protected

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

Property default_kinematics_cache_ptr_with_gradients
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: