Drake
 All Classes Files Functions Variables
FrictionConeWrench Class Reference

constrain the friction force to be within a friction cone. More...

Inheritance diagram for FrictionConeWrench:
Collaboration diagram for FrictionConeWrench:

Public Member Functions

function FrictionConeWrench (robot, body, body_pts, FC_mu, FC_axis, force_normalize_factor)
 
function evalWrenchConstraint (obj, kinsol, F, slack)
 This function evaluates the constraint and its non-zero entries in the sparse gradient matrix. More...
 
function force (obj)
 Compute the indivisual force from the force parameters F. More...
 
function torque (obj)
 Compute the torque at each contact point from the force parameter. More...
 
function contactPosition (obj, kinsol)
 Returns the contact positions and its gradient w.r.t q. More...
 
- Public Member Functions inherited from RigidBodyContactWrench
function RigidBodyContactWrench (robot, body, body_pts)
 
function generateWrenchConstraint (obj)
 

Protected Member Functions

function generateWrenchLincon (obj)
 
- Protected Member Functions inherited from RigidBodyContactWrench
function checkForceSize (obj, F)
 

Protected Attributes

Property normal_dir
 
Property FC_mu
 
Property FC_axis
 of the friction cone at contact point body_pts(:,i) More...
 
Property force_normalize_factor
 friction cone at the contact point body_pts(:,i). FC_axis(:,i) is in the world frame More...
 
Property cos_theta_square
 where force parameters are the arguments being constrained More...
 
- Protected Attributes inherited from RigidBodyContactWrench
Property robot
 
Property body
 
Property body_name
 
Property body_pts
 
Property num_pts
 of the contact point on the body frame. More...
 
Property num_wrench_constraint
 
Property num_pt_F
 
Property F_lb
 
Property F_ub
 force parameters More...
 
Property wrench_iCfun
 force parameters More...
 
Property wrench_jCvar
 
Property wrench_cnstr_ub
 
Property wrench_cnstr_lb
 
Property wrench_cnstr_name
 
Property contact_force_type
 
Property num_slack
 
Property slack_name
 
Property slack_lb
 
Property slack_ub
 
Property kinematics_chain_idx
 
Property complementarity_flag
 

Additional Inherited Members

- Public Attributes inherited from RigidBodyContactWrench
Constant Property FrictionConeType
 
Constant Property LinearFrictionConeType
 
Constant Property GraspType
 
Constant Property GraspFrictionConeType
 

Detailed Description

constrain the friction force to be within a friction cone.

Constructor & Destructor Documentation

function FrictionConeWrench ( robot  ,
body  ,
body_pts  ,
FC_mu  ,
FC_axis  ,
force_normalize_factor   
)
Parameters
robotA RigidBodyManipulator or a TimeSteppingRigidBodyManipulator object
body– The index of contact body on the robot
body_pts– A 3 x num_pts double matrix, each column represents the coordinate of the contact point on the body frame.
FC_mu– A 1 x num_pts double vector or a scalar, FC_mu(i) is the friction coefficient of the friction cone at contact point body_pts(:,i). If FC_mu is a scalar, then that friciton coefficient is used for every contact point
FC_axis– A 3 x num_pts double matrix or a 3 x 1 vector, FC_axis(:,i) is the axis of the friction cone at the contact point body_pts(:,i). FC_axis(:,i) is in the world frame. If FC_axis is a 3 x 1 vector, then that axis is used for every friction cone
force_normalize_factor– A positive double scalar. The actual force is force_normalize_factor*force_parameter, where force_parameters are the argument to the constraint. Default value is robot_mass*g
Return values
obj

Member Function Documentation

function contactPosition ( obj  ,
kinsol   
)
virtual

Returns the contact positions and its gradient w.r.t q.

Parameters
kinsol– The kinematics tree
Return values
pos– A matrix with 3 rows. pos(:,i) is the i'th contact position
J– A matrix of size prod(size(pos)) x nq. The gradient of pos w.r.t q

Reimplemented from RigidBodyContactWrench.

function evalWrenchConstraint ( obj  ,
kinsol  ,
,
slack   
)
virtual

This function evaluates the constraint and its non-zero entries in the sparse gradient matrix.

Parameters
t- A scalar, the time to evaluate friction cone constraint
kinsol- kinematics tree returned from doKinematics function
F- A 3 x num_pts double matrix. The contact forces
Return values
c- A num_pts x 1 double vector, the constraint value
dc- A obj.num_pts x (3*obj.num_pts) double matrix. The gradient of c w.r.t F

Reimplemented from RigidBodyContactWrench.

function force ( obj  )
virtual

Compute the indivisual force from the force parameters F.

The individual forces are reshape(A*F,3,obj.num_pts)

Return values
A– A (3*num_pts) x (obj.num_pt_F*obj.num_pts) double matrix

Reimplemented from RigidBodyContactWrench.

function generateWrenchLincon ( obj  )
protectedvirtual
Return values
lincon

Reimplemented from RigidBodyContactWrench.

function torque ( obj  )
virtual

Compute the torque at each contact point from the force parameter.

Return values
A– A (3*num_pts) x (obj.num_pt_F*obj.num_pts) double matrix. reshape(A*F(:),3,num_pts) are the torque at each contact point

Reimplemented from RigidBodyContactWrench.

Member Data Documentation

Property cos_theta_square
protected

where force parameters are the arguments being constrained

Property FC_axis
protected

of the friction cone at contact point body_pts(:,i)

Property FC_mu
protected
Property force_normalize_factor
protected

friction cone at the contact point body_pts(:,i). FC_axis(:,i) is in the world frame

Property normal_dir
protected

The documentation for this class was generated from the following file: