Classes
class	BrickStaticEquilibriumNonlinearConstraint
	Given the set of contacts between the fingertips and the brick, impose the static equilibrium as a nonlinear constraint, that the total force/torque applied on the brick is 0. More...

class	GripperBrickHelper
	The helper class that contains the diagram of the planar gripper (3 planar fingers) with a brick. More...

class	GripperCommandDecoder
	Handles lcmt_planar_gripper_command messages from a LcmSubscriberSystem. More...

class	GripperCommandEncoder
	Creates and outputs lcmt_planar_gripper_command messages. More...

class	GripperStatusDecoder
	Handles lcmt_planar_gripper_status messages from a LcmSubscriberSystem. More...

class	GripperStatusEncoder
	Creates and outputs lcmt_planar_gripper_status messages. More...

class	PlanarManipulandStatusDecoder
	Handles lcmt_planar_manipuland_status messages from a LcmSubscriberSystem. More...

class	PlanarManipulandStatusEncoder
	Creates and outputs lcmt_planar_manipuland_status messages. More...

Enumerations
enum	Finger { kFinger1, kFinger2, kFinger3 }

enum	BrickFace { kPosZ, kNegZ, kPosY, kNegY }

Functions
Matrix2X< symbolic::Variable >	AddBrickStaticEquilibriumConstraint (const GripperBrickHelper< double > &gripper_brick_system, const std::vector< std::pair< Finger, BrickFace >> &finger_face_contacts, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_vars, systems::Context< double > plant_mutable_context, solvers::MathematicalProgram prog)
	Given the assignment of contacts between fingers and faces, add the following constraints/variables to the program: More...

std::string	to_string (Finger finger)

template<typename T >
void	AddFrictionConeConstraint (const GripperBrickHelper< T > &gripper_brick, Finger finger, BrickFace brick_face, const Eigen::Ref< const Vector2< symbolic::Variable >> &f_Cb_B, double friction_cone_shrink_factor, solvers::MathematicalProgram *prog)
	Adds the friction cone as linear constraints on the contact force f_Cb_B (the contact force applied on the brick contact point Cb, expressed in the brick frame B). More...

void	AddFingerTipInContactWithBrickFaceConstraint (const GripperBrickHelper< double > &gripper_brick_system, Finger finger, BrickFace brick_face, solvers::MathematicalProgram prog, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_vars, systems::Context< double > plant_context, double face_shrink_factor, double depth)
	Add the kinematic constraint that the finger tip (the sphere collision geometry in the tip of the finger) is in contact with a shrunk region on the brick face. More...

Eigen::Vector3d	ComputeFingerTipInBrickFrame (const GripperBrickHelper< double > &gripper_brick, const Finger finger, const systems::Context< double > &plant_context, const Eigen::Ref< const Eigen::VectorXd > &q)

Vector3< AutoDiffXd >	ComputeFingerTipInBrickFrame (const GripperBrickHelper< double > &gripper_brick, const Finger finger, const systems::Context< double > &plant_context, const Eigen::Ref< const AutoDiffVecXd > &q)

void	AddFingerNoSlidingConstraint (const GripperBrickHelper< double > &gripper_brick, Finger finger, BrickFace face, double rolling_angle_bound, solvers::MathematicalProgram prog, systems::Context< double > from_context, systems::Context< double > *to_context, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_from, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_to, double face_shrink_factor, double depth)
	Impose a kinematic constraint which prohibits the fingertip sphere from sliding on the brick's face. More...

void	AddFingerNoSlidingFromFixedPostureConstraint (const GripperBrickHelper< double > &gripper_brick, Finger finger, BrickFace face, double rolling_angle_lower, double rolling_angle_upper, solvers::MathematicalProgram prog, const systems::Context< double > &from_fixed_context, systems::Context< double > to_context, const Eigen::Ref< const VectorX< symbolic::Variable >> &q_to, double face_shrink_factor, double depth)
	Impose the kinematic constraint that the finger can only roll (or stick) starting from a given fixed posture. More...

template<typename T >
void	WeldGripperFrames (MultibodyPlant< T > *plant)
	Welds each finger's base frame to the world. More...

std::pair< MatrixX< double >, std::map< std::string, int > >	ParseKeyframes (const std::string &name, EigenPtr< Vector3< double >> brick_initial_pose=EigenPtr< Vector3< double >>(nullptr))
	Parses a text file containing keyframe joint positions for the planar gripper and the planar brick (the object being manipulated). More...

MatrixX< double >	ReorderKeyframesForPlant (const MultibodyPlant< double > &plant, const MatrixX< double > keyframes, std::map< std::string, int > *finger_joint_name_to_row_index_map)
	Reorders the joint keyframe matrix data contained in `keyframes` such that joint keyframes (rows) are ordered according to the `plant`'s joint velocity index ordering, making it compatible with the inverse dynamics controller's desired state input port ordering. More...

const math::RigidTransformd	X_WGripper ()
	Returns the planar gripper frame G's transform w.r.t. the world frame W. More...

Variables
constexpr int	kNumFingers = 3

constexpr int	kNumJoints = kNumFingers * 2

constexpr int	kGripperDefaultNumFingers = 3

constexpr double	kGripperLcmStatusPeriod = 0.010

constexpr double	kPlanarManipulandStatusPeriod = 0.010

Enumeration Type Documentation

◆ BrickFace

enum BrickFace

strong

Enumerator
kPosZ
kNegZ
kPosY
kNegY

◆ Finger

enum Finger

strong

Enumerator
kFinger1
kFinger2
kFinger3

Function Documentation

◆ AddBrickStaticEquilibriumConstraint()

Matrix2X<symbolic::Variable> drake::examples::planar_gripper::AddBrickStaticEquilibriumConstraint	(	const GripperBrickHelper< double > &	gripper_brick_system,
		const std::vector< std::pair< Finger, BrickFace >> &	finger_face_contacts,
		const Eigen::Ref< const VectorX< symbolic::Variable >> &	q_vars,
		systems::Context< double > *	plant_mutable_context,
		solvers::MathematicalProgram *	prog
	)

Given the assignment of contacts between fingers and faces, add the following constraints/variables to the program:

Decision variables representing the finger contact force on the brick, expressed in the brick frame.
The static equilibrium constraint ensuring that the total wrench on the brick is 0.
The constraint that the contact force is within a friction cone.

◆ AddFingerNoSlidingConstraint()

void drake::examples::planar_gripper::AddFingerNoSlidingConstraint	(	const GripperBrickHelper< double > &	gripper_brick,
		Finger	finger,
		BrickFace	face,
		double	rolling_angle_bound,
		solvers::MathematicalProgram *	prog,
		systems::Context< double > *	from_context,
		systems::Context< double > *	to_context,
		const Eigen::Ref< const VectorX< symbolic::Variable >> &	q_from,
		const Eigen::Ref< const VectorX< symbolic::Variable >> &	q_to,
		double	face_shrink_factor,
		double	depth
	)

Impose a kinematic constraint which prohibits the fingertip sphere from sliding on the brick's face.

That is, the fingertip sphere is only allowed to roll on the brick's surface. Note that zero rolling (i.e., sticking) is also allowed.

Parameters

gripper_brick	Contains the gripper brick diagram.
finger	The finger that should not slide.
face	The brick face that the finger sticks to (or rolls on).
rolling_angle_bound	The non-negative bound on the rolling angle (in radians).
prog	The optimization program to which the constraint is added.
from_context	The context that contains the posture of the plant, before rolling occurs.
to_context	The context that contains the posture of the plant, after rolling occurs.
q_from	The variable representing the "from" posture.
q_to	The variable representing the "to" posture.

◆ AddFingerNoSlidingFromFixedPostureConstraint()

void drake::examples::planar_gripper::AddFingerNoSlidingFromFixedPostureConstraint	(	const GripperBrickHelper< double > &	gripper_brick,
		Finger	finger,
		BrickFace	face,
		double	rolling_angle_lower,
		double	rolling_angle_upper,
		solvers::MathematicalProgram *	prog,
		const systems::Context< double > &	from_fixed_context,
		systems::Context< double > *	to_context,
		const Eigen::Ref< const VectorX< symbolic::Variable >> &	q_to,
		double	face_shrink_factor,
		double	depth
	)

Impose the kinematic constraint that the finger can only roll (or stick) starting from a given fixed posture.

The angle of the finger is defined as the pitch angle of the finger link 2 (about +x axis). The change of finger angles from the starting posture to the ending posture is limited to be within rolling_angle_upper and rolling_angle_lower. If you want to impose sticking contact (no rolling), then set rolling_angle_lower = rolling_angle_upper = 0.

Parameters

gripper_brick	Contains the gripper brick diagram.
finger	The finger that should not slide.
face	The brick face that the finger sticks to (or rolls on).
rolling_angle_lower	The lower bound on the rolling angle.
rolling_angle_upper	The upper bound on the rolling angle.
prog	The optimization program to which the constraint is added.
from_context	The context containing posture, from which the finger should stick to (or roll).
to_context	The context that contains the value of the posture after rolling (sticking).
q_to	The variable representing the "to" posture.
face_shrink_factor	A factor < 1, indicates the region on the face that the finger can roll within.

◆ AddFingerTipInContactWithBrickFaceConstraint()

void drake::examples::planar_gripper::AddFingerTipInContactWithBrickFaceConstraint	(	const GripperBrickHelper< double > &	gripper_brick_system,
		Finger	finger,
		BrickFace	brick_face,
		solvers::MathematicalProgram *	prog,
		const Eigen::Ref< const VectorX< symbolic::Variable >> &	q_vars,
		systems::Context< double > *	plant_context,
		double	face_shrink_factor,
		double	depth
	)

Add the kinematic constraint that the finger tip (the sphere collision geometry in the tip of the finger) is in contact with a shrunk region on the brick face.

Parameters

gripper_brick_system	The gripper brick system on which the constraint is imposed.
finger	The finger in contact.
brick_face	The face of the brick in contact.
prog	The program to which the constraint is added.
q_vars	The variable for the configuration.
plant_context	The context containing the value of q_vars.
face_shrink_factor	A factor to determine the shrunk region on each face. If face_shrink_factor = 1, then the region includes the whole face, 0 < face_shrink_factor < 1 corresponds to the scaled region of the face, if face_shrink_factor = 0, then the region shrinks to the singleton centroid.
depth	The penetration depth between the finger tip sphere and the brick.

◆ AddFrictionConeConstraint()

void drake::examples::planar_gripper::AddFrictionConeConstraint	(	const GripperBrickHelper< T > &	gripper_brick,
		Finger	finger,
		BrickFace	brick_face,
		const Eigen::Ref< const Vector2< symbolic::Variable >> &	f_Cb_B,
		double	friction_cone_shrink_factor,
		solvers::MathematicalProgram *	prog
	)

Adds the friction cone as linear constraints on the contact force f_Cb_B (the contact force applied on the brick contact point Cb, expressed in the brick frame B).

Notice that since the example is planar, we can write this friction cone as linear constraints, as opposed to the general nonlinear constraint in StaticFrictionConeConstraint.

Parameters

gripper_brick	The planar gripper system manipulating a brick.
finger	The finger in contact with the brick face.
brick_face	The contact facet on the brick.
f_Cb_B	The contact force applied on the brick contact point Cb, expressed in the brick frame B.
friction_cone_shrink_factor	The factor to shrink the friction coefficient mu in the planning.
prog	The program to which the constraint is added.

◆ ComputeFingerTipInBrickFrame() [1/2]

Eigen::Vector3d drake::examples::planar_gripper::ComputeFingerTipInBrickFrame	(	const GripperBrickHelper< double > &	gripper_brick,
		const Finger	finger,
		const systems::Context< double > &	plant_context,
		const Eigen::Ref< const Eigen::VectorXd > &	q
	)

◆ ComputeFingerTipInBrickFrame() [2/2]

Vector3<AutoDiffXd> drake::examples::planar_gripper::ComputeFingerTipInBrickFrame	(	const GripperBrickHelper< double > &	gripper_brick,
		const Finger	finger,
		const systems::Context< double > &	plant_context,
		const Eigen::Ref< const AutoDiffVecXd > &	q
	)

◆ ParseKeyframes()

std::pair<MatrixX<double>, std::map<std::string, int> > drake::examples::planar_gripper::ParseKeyframes	(	const std::string &	name,
		EigenPtr< Vector3< double >>	brick_initial_pose = `EigenPtr< Vector3< double >>(nullptr)`
	)

Parses a text file containing keyframe joint positions for the planar gripper and the planar brick (the object being manipulated).

Parameters

[in]	name	The file name to parse.
[out]	brick_initial_pose	A vector containing the initial brick pose, expressed in the gripper frame G.

Returns: A std::pair containing a matrix of finger joint position keyframes (each matrix column represents a single keyframe containing values for all joint positions) and a std::map containing the mapping between each finger joint name and the corresponding row index in the keyframe matrix containing the data for that joint.

Precondition: The file should begin with a header row that indicates the joint ordering for keyframes. Header names should consist of three finger base joints, three finger mid joints, and three brick joints (9 total): {finger1_BaseJoint, finger2_BaseJoint, finger3_BaseJoint, finger1_MidJoint, finger2_MidJoint, finger3_MindJoint, brick_translate_y_joint, brick_translate_z_joint, brick_revolute_x_joint}. Note that brick translations should be expressed in the planar-gripper frame G. Names may appear in any order. Each row (keyframe) following the header should contain the same number of values as indicated in the header. All entries should be white space delimited and the file should end in a newline character. The behavior of parsing is undefined if these conditions are not met.

◆ ReorderKeyframesForPlant()

MatrixX<double> drake::examples::planar_gripper::ReorderKeyframesForPlant	(	const MultibodyPlant< double > &	plant,
		const MatrixX< double >	keyframes,
		std::map< std::string, int > *	finger_joint_name_to_row_index_map
	)

Reorders the joint keyframe matrix data contained in keyframes such that joint keyframes (rows) are ordered according to the plant's joint velocity index ordering, making it compatible with the inverse dynamics controller's desired state input port ordering.

The incoming plant is the MultibodyPlant used for inverse dynamics control, i.e., the "control plant". The number of planar-gripper joints kNumJoints must exactly match plant.num_positions().

Parameters

[in]	plant	The MultibodyPlant providing the velocity index ordering.
[in]	keyframes	The planar gripper keyframes.
[out]	finger_joint_name_to_row_index_map	A std::map which contains the incoming joint name to row index ordering. This map is updated to reflect the new keyframe reordering.

Returns: A MatrixX containing the reordered keyframes.

Exceptions

If	the number of keyframe rows does not match the size of `finger_joint_name_to_row_index_map`
If	the number of keyframe rows does not match the number of planar-gripper joints.
If	`kNumJoints` does not exactly match plant.num_positions().

◆ to_string()

std::string drake::examples::planar_gripper::to_string ( Finger finger )

◆ WeldGripperFrames()

void drake::examples::planar_gripper::WeldGripperFrames ( MultibodyPlant< T > * plant )

Welds each finger's base frame to the world.

The planar-gripper is made up of three 2-DOF fingers, whose bases are fixed equidistant along the perimeter of a circle. The origin (Go) of the planar gripper is located at the center of the workspace, i.e., if all joints are set to zero and all fingers are pointing inwards, the origin is the point that is equidistant to all fingertips. This method welds the planar-gripper such that all motion lies in the Y-Z plane (in frame G). Note: The planar gripper frame G perfectly coincides with the world coordinate frame W when welded via this method.

Parameters

plant The plant containing the planar-gripper.

Template Parameters

T	The scalar type, which must be `double`.

◆ X_WGripper()

const math::RigidTransformd drake::examples::planar_gripper::X_WGripper ( )

Returns the planar gripper frame G's transform w.r.t. the world frame W.

Variable Documentation

◆ kGripperDefaultNumFingers

constexpr int kGripperDefaultNumFingers = 3

◆ kGripperLcmStatusPeriod

constexpr double kGripperLcmStatusPeriod = 0.010

◆ kNumFingers

constexpr int kNumFingers = 3

◆ kNumJoints

constexpr int kNumJoints = kNumFingers * 2

◆ kPlanarManipulandStatusPeriod

constexpr double kPlanarManipulandStatusPeriod = 0.010

Classes

Enumerations

Functions

Variables

Enumeration Type Documentation

◆ BrickFace

◆ Finger

Function Documentation

◆ AddBrickStaticEquilibriumConstraint()

◆ AddFingerNoSlidingConstraint()

◆ AddFingerNoSlidingFromFixedPostureConstraint()

◆ AddFingerTipInContactWithBrickFaceConstraint()

◆ AddFrictionConeConstraint()

◆ ComputeFingerTipInBrickFrame() [1/2]

◆ ComputeFingerTipInBrickFrame() [2/2]

◆ ParseKeyframes()

◆ ReorderKeyframesForPlant()

◆ to_string()

◆ WeldGripperFrames()

◆ X_WGripper()

Variable Documentation

◆ kGripperDefaultNumFingers

◆ kGripperLcmStatusPeriod

◆ kNumFingers

◆ kNumJoints

◆ kPlanarManipulandStatusPeriod