KrisLibrary  1.0.0
Files | Classes | Typedefs | Functions
Kinematics

Classes to define robot kinematics. More...

Files

file  AngleBracket.h
 
file  ConstrainedDynamics.h
 Functions for computing dynamics under constraints.
 
file  Geometry.h
 Basic geometric utilities for rotations, intersections.
 
file  IKFunctions.h
 Helper classes and functions for solving iterative inverse kinematics problems.
 
file  RobotKinematics3D.h
 
file  Rotation.h
 Utility functions for converting between different rotation representations and computing derivatives.
 

Classes

struct  AnalyticIKSolution
 A structure containing computed analytic IK solutions. More...
 
class  AnalyticalIKSolver
 Hook for an analytic inverse kinematics solver. The Solve method must be defined by the subclass. More...
 
class  CartesianDriveSolver
 Simultaneously moves joints on the robot to achieve one or more Cartesian velocity goals. A highly configurable and reliable solver suitable for long-duration, precise cartesian motions. More...
 
class  Chain
 A tree-based chain structure. More...
 
struct  IKGoal
 A structure defining a link's desired configuration for IK. More...
 
struct  RobotIKFunction
 A vector field function class defining C(q)=0 for IK. More...
 
struct  RobotIKSolver
 A Newton-Raphson robot IK solver. More...
 
struct  WorldPositionFunction
 Function class that returns the world-space position of a point on the robot. More...
 
struct  IKGoalFunction
 Function class that returns the position/rotation errors associated with a given IKGoal. More...
 
struct  RobotCOMFunction
 Function class that measures the difference between the robot's center of mass and a desired COM. More...
 
struct  JointStructure
 Calculates workspace bounds for a robot with constrained links. More...
 
struct  NewtonEulerSolver
 The Featherstone algorithm for O(n) computation of either joint torques from accelerations, or accelerations from torques. More...
 
class  RLG
 A sampler for closed-chain kinematics that takes joint workspace bounds into account. More...
 
class  FreeRLG
 RLG that sets a virtual linkage of free dof's using analytic IK. More...
 
class  RobotDynamics3D
 Class defining kinematic and dynamic state of a robot, with commonly used functions. Inherits from RobotKinematics3D. More...
 
class  RobotKinematics3D
 Class defining kinematics of a robot, and commonly used functions. More...
 
class  RobotLink3D
 Kinematic and physical parameters of a link. More...
 
struct  DirectionCosines
 9-D rotation parameterization that lays out the columns of the rotation matrix in a single vector. More...
 
struct  AxisSweptPoint
 Parameterizes a circle in space by sweeping a point around a line. More...
 
struct  Arc3D
 An arc segment of a circle in space. More...
 
struct  HollowBall
 A 3-D annulus innerRadius <= |x-center| <= outerRadius. More...
 
struct  WorkspaceBound
 Bounds the workspace of a robot linkage with a simple bound. More...
 

Typedefs

typedef Vector Config
 an alias for Vector
 

Functions

AngleInterval AngleBracket_1D_Disk (Real p, Real c, Real r)
 
AngleInterval AngleBracket_2D_Disk (const Vector2 &p, const Vector2 &c, Real r)
 Full 2D version.
 
AngleInterval AngleBracket_3D_Ball (const Vector3 &p, const Vector3 &c, Real r)
 
AngleInterval AngleBracket_3D_Ball (const Vector3 &p, const Vector3 &w, const Vector3 &c, Real r)
 same as above, but axis w is arbitrary
 
AngleInterval AngleBracket_3D_Ball (const Vector3 &p, int axis, const Vector3 &c, Real r)
 same as above, but axis can be x,y,or z axis (x=0,y=1,z=2)
 
bool ConstrainedCalcAccel (RobotDynamics3D &robot, const std::vector< int > &fixedLinks, const std::vector< int > &fixedDofs, const Vector &t, Vector &ddq, Vector *f=NULL)
 If the fixed links are fixed in place, computes how torques t map to accelerations ddq. Assumes q,dq are given in the robot and that the robot's frames are updated. Provide f if you want the constraint forces.
 
bool ConstrainedCalcAccel (RobotDynamics3D &robot, const std::vector< IKGoal > &constraints, const Vector &t, Vector &ddq, Vector *f=NULL)
 If the IK constraints are fixed in place, computes how torques t map to accelerations ddq. Assumes q,dq are given in the robot and that the robot's frames are updated. Provide f if you want the constraint forces.
 
bool ConstrainedCalcAccel (RobotDynamics3D &robot, const Vector &ddx, const Matrix &dC_dq, const Vector &t, Vector &ddq, Vector *f=NULL)
 If the constraint ddx=dC/dq(q)ddq is given, computes how the torques t map to accelerations ddq. The jacobian dC/dq(q) is given in the dC_dq param. Assumes q,dq are given in the robot and that the robot's frames are updated. Provide f if you want the constraint forces.
 
bool ConstrainedForwardDynamics (RobotDynamics3D &robot, const std::vector< int > &fixedLinks, const std::vector< int > &fixedDofs, Matrix &A, Vector &b)
 Returns a linearized dynamic model onto the constraints, such that if t is the torque applied to the system, the acceleration of the constrained system is ddq=A*t+b.
 
bool ConstrainedForwardDynamics (RobotDynamics3D &robot, const Vector &ddx, const Matrix &dC_dq, Matrix &A, Vector &b)
 Returns a linearized dynamic model onto the constraints, such that if t is the torque applied to the system, the acceleration of the constrained system is ddq=A*t+b.
 
bool ConstrainedProjector (RobotDynamics3D &robot, const std::vector< int > &fixedLinks, const std::vector< int > &fixedDofs, Matrix &A, Vector &b)
 Returns a projection of the dynamics onto the constraints, such that if ddq0 is the acceleration of the unconstrained system, then the acceleration of the constrained system is ddq=A*ddq0+b.
 
bool ConstrainedProjector (RobotDynamics3D &robot, const Vector &ddx, const Matrix &dC_dq, Matrix &A, Vector &b)
 Returns a projection of the dynamics onto the constraints ddx=dC/dq(q)*ddq, such that if ddq0 is the acceleration of the unconstrained system, then the acceleration of the constrained system is ddq=A*ddq0+b.
 
template<class Rot >
void GetRotationAboutLocalPoint (const Vector3 &localpt, const Vector3 &pt, const Rot &r, RigidTransform &T)
 
template<class Rot >
void GetRotationAboutPoint (const Vector3 &pt, const Rot &r, RigidTransform &T)
 Returns the RigidTransform that rotates by r about the point pt.
 
bool BallCircleCollision (const Sphere3D &a, const Circle3D &b)
 Returns true if the solid ball a intersects the circle boundary b.
 
void CircleCircleClosestPoints (const Circle3D &a, const Circle3D &b, Vector3 &pa, Vector3 &pb)
 Calculates the closest points on the boundaries of circles a,b.
 
int BallBallIntersection (const Sphere3D &a, const Sphere3D &b, Circle3D &c)
 Calculates the intersection region of the balls a,b. More...
 
void CollisionSelfTest ()
 
void GetDefaultIKDofs (const RobotKinematics3D &robot, const std::vector< IKGoal > &ik, ArrayMapping &m)
 
void GetPassiveChainDOFs (const RobotKinematics3D &robot, const IKGoal &ikGoal, ArrayMapping &passiveDofs)
 
void GetPassiveChainDOFs (const RobotKinematics3D &robot, int link, int numTerms, ArrayMapping &passiveDofs)
 
bool SolveIK (RobotIKFunction &f, Real tolerance, int &iters, int verbose=1)
 
bool SolveIK (RobotKinematics3D &, const std::vector< IKGoal > &problem, Real tolerance, int &iters, int verbose=1)
 Same as above, constructing the ik function from an IK problem.
 
bool SolveIK (RobotKinematics3D &, const std::vector< IKGoal > &problem, const std::vector< int > &activeDofs, Real tolerance, int &iters, int verbose=1)
 
bool SolvePassiveChainIK (RobotKinematics3D &, const IKGoal &goal, Real tolerance, int &iters, int verbose=0)
 
void EvalIKError (const IKGoal &g, const RigidTransform &T, Real *poserr, Real *orierr)
 For the goal link's transformation T, computes the error of the IK goal g.
 
void EvalIKError (const IKGoal &g, const RigidTransform &T, Vector &err)
 
Real RobotIKError (const RobotKinematics3D &robot, const IKGoal &goal)
 
void EvalIKGoalDeriv (const IKGoal &g, const RigidTransform &T, const Vector3 &dw, const Vector3 &dv, Vector &derr)
 
bool IntersectGoals (const IKGoal &a, const IKGoal &b, IKGoal &c, Real tolerance=1e-5)
 
bool AddGoalNonredundant (const IKGoal &goal, std::vector< IKGoal > &goalSet, Real tolerance=1e-5)
 
bool IsReachableGoal (const RobotKinematics3D &, const IKGoal &a, const IKGoal &b)
 
bool IsReachableGoal (const IKGoal &a, const IKGoal &b, Real jointDistance)
 
bool GetRotationCenter (const RigidTransform &T, Vector3 &p)
 
void GetMinimalRotation (const Vector3 &x, const Vector3 &y, Matrix3 &R)
 
void GetMinimalRotationToPlane (const Vector3 &x, const Vector3 &y, Matrix3 &R)
 
void MatrixDerivative (const Matrix3 &R, const Vector3 &z, Matrix3 &dR)
 Derivative dR of the rotation matrix R of a frame as it rotates about the axis z. More...
 
void DirectionCosinesDerivative (const Vector &R, const Vector3 &z, Vector &dR)
 
void MomentDerivative (const Matrix3 &R, const Vector3 &z, Vector3 &dm)
 
void MomentDerivative (const MomentRotation &m, const Vector3 &z, Vector3 &dm)
 Same as above, but specifies R with the moment m.
 
void MomentDerivative (const Vector3 &m, const Matrix3 &R, const Vector3 &z, Vector3 &dm)
 Same as above, but both m and R are provided.
 
void QuaternionDerivative (const Matrix3 &R, const Vector3 &z, Quaternion &dq)
 
bool EulerAngleDerivative (const Vector3 &theta, const Vector3 &z, int u, int v, int w, Vector3 &dtheta)
 Calculates the derivative of the euler angle representation of R as it rotates around the axis z. More...
 
void AngularVelocityEulerAngle (const Vector3 &theta, const Vector3 &dtheta, int u, int v, int w, Vector3 &z)
 Calculates the angular velocity z of the rotation described by theta given its time derivatives dtheta. More...
 
void EulerAngleMoments (const Vector3 &theta, int u, int v, int w, Matrix3 &A)
 
Real MatrixAbsoluteAngle (const Matrix3 &R)
 Returns the absolute angle of the rotation R. More...
 
Real MatrixAngleAboutAxis (const Matrix3 &R, const Vector3 &a)
 Returns the angle that R makes about the axis a. More...
 
Real MatrixAngleDerivative (const Matrix3 &R, const Vector3 &z)
 Derivative dtheta/dz, the change in the absolute angle of rotation R as it rotates about the axis z. More...
 
void MatrixDerivative2 (const Matrix3 &R, const Vector3 &w, const Vector3 &a, Matrix3 &ddR)
 Second derivative ddR of the rotation matrix R of a frame as it rotates with velocity w, acceleration a about the axis z. More...
 
void ForwardDifferenceAngularVelocity (const Matrix3 &R0, const Matrix3 &R1, Real h, Vector3 &z)
 Computes the angular velocity z at time 0, given current rotation and rotation at next time step. More...
 
void CenteredDifferenceAngularVelocity (const Matrix3 &R_1, const Matrix3 &R0, const Matrix3 &R1, Real h, Vector3 &z)
 Computes the angular velocity z at time 0, given current rotation, and rotations at previous/next time steps. More...
 
void CenteredDifferenceAngularAccel (const Matrix3 &R_1, const Matrix3 &R0, const Matrix3 &R1, Real h, Vector3 &a)
 Computes the angular acceleration a at time 0, given current rotation, and rotations at previous/next time steps. More...
 
Real AxisRotationMagnitude (const Matrix3 &R, const Vector3 &a)
 Computes the "amount" of rotation about the axis a that thegiven rotation matrix R performs. Specifically, finds the angle theta such that e^[theta*a] minimizes the angular distance to R. More...
 
void NormalizeRotation (Matrix3 &R)
 For a not-quite rotation matrix R, replaces it with the closest rotation matrix. I.e., find an orthogonal matrix that minimizes the distance to R.
 

Detailed Description

Classes to define robot kinematics.

Function Documentation

bool AddGoalNonredundant ( const IKGoal goal,
std::vector< IKGoal > &  goalSet,
Real  tolerance = 1e-5 
)

Adds a new constraint to a goalSet while merging it in intelligently to minimize the number of resulting constraints. False is returned if goal is incompatible with the previous goals.

References IKGoal::destLink, IntersectGoals(), and IKGoal::link.

AngleInterval AngleBracket_1D_Disk ( Real  p,
Real  c,
Real  r 
)

Simplest form, assumes p and the disk D lie on the x axis (Actually in 2d) Point is (p,0). Disk's center is (c,0), radius is r.

References Math::AngleInterval::setRange().

Referenced by AngleBracket_2D_Disk().

AngleInterval AngleBracket_3D_Ball ( const Vector3 p,
const Vector3 c,
Real  r 
)

3D version p rotates around the z axis. The ball B is centered at c, with radius r.

References AngleBracket_2D_Disk(), and Math::pythag_leg().

Referenced by AngleBracket_3D_Ball().

void AngularVelocityEulerAngle ( const Vector3 theta,
const Vector3 dtheta,
int  u,
int  v,
int  w,
Vector3 z 
)

Calculates the angular velocity z of the rotation described by theta given its time derivatives dtheta.

R is specified with the euler angles theta=(t1,t2,t3), which rotate about the axes indexed by u,v,w in order. i.e. R=Ru(t1)Rv(t2)Rw(t3) where indices 0,1,2 indicate axes (x,y,z).

References EulerAngleMoments().

Real AxisRotationMagnitude ( const Matrix3 R,
const Vector3 a 
)

Computes the "amount" of rotation about the axis a that thegiven rotation matrix R performs. Specifically, finds the angle theta such that e^[theta*a] minimizes the angular distance to R.

E.g., if R=e^[u*a], it will return u. However, if the axis of R is not aligned along a, this function will return something sensible.

Referenced by CartesianDriveSolver::Drive().

int BallBallIntersection ( const Sphere3D a,
const Sphere3D b,
Circle3D c 
)

Calculates the intersection region of the balls a,b.

Returns the dimension of the intersection
0 - no intersection
1 - point (region returned as the center of c)
2 - circle (region returned as c)
3 - ball (a is inside b)
4 - ball (b is inside a)

References BallCircleCollision(), CircleCircleClosestPoints(), Math::FuzzyEquals(), and Math::pythag_leg().

void CenteredDifferenceAngularAccel ( const Matrix3 R_1,
const Matrix3 R0,
const Matrix3 R1,
Real  h,
Vector3 a 
)

Computes the angular acceleration a at time 0, given current rotation, and rotations at previous/next time steps.

R0=R(0), R_1=R(-h), R1=R(h), h is the time step.

References CenteredDifferenceAngularVelocity(), Math3D::Matrix3::getCrossProduct(), and Math3D::Matrix3::setCrossProduct().

void CenteredDifferenceAngularVelocity ( const Matrix3 R_1,
const Matrix3 R0,
const Matrix3 R1,
Real  h,
Vector3 z 
)

Computes the angular velocity z at time 0, given current rotation, and rotations at previous/next time steps.

R0=R(0), R_1=R(-h), R1=R(h), h is the time step.

References Math3D::Matrix3::getCrossProduct().

Referenced by CenteredDifferenceAngularAccel().

void DirectionCosinesDerivative ( const Vector &  R,
const Vector3 z,
Vector &  dR 
)

Same as above, but R and dR are parameterized by direction cosines.

bool EulerAngleDerivative ( const Vector3 theta,
const Vector3 z,
int  u,
int  v,
int  w,
Vector3 dtheta 
)

Calculates the derivative of the euler angle representation of R as it rotates around the axis z.

R is specified with theta= (t1,t2,t3), which rotate about the axes indexed by u,v,w in order. i.e. R=Ru(t1)Rv(t2)Rw(t3) where indices 0,1,2 indicate axes (x,y,z). False may be returned if the euler angles are singular.

References EulerAngleMoments().

void EulerAngleMoments ( const Vector3 theta,
int  u,
int  v,
int  w,
Matrix3 A 
)

Calculates the rotation moment for each euler angle, returns them in the 3 columns of A. Rotation matrix R specified as above.

References Math3D::Matrix3::setRotateX(), Math3D::Matrix3::setRotateY(), and Math3D::Matrix3::setRotateZ().

Referenced by AngularVelocityEulerAngle(), and EulerAngleDerivative().

void EvalIKGoalDeriv ( const IKGoal g,
const RigidTransform T,
const Vector3 dw,
const Vector3 dv,
Vector &  derr 
)

For the goal link's transformaiton T, with rotational velocity dw and linear velocity dv, computes the derivative of g's error

References IKGoal::direction, Math::dot(), IKGoal::endPosition, IKGoal::endRotation, Math3D::GetCanonicalBasis(), Math3D::Vector3::getOrthogonalBasis(), Math::IsFinite(), IKGoal::localAxis, MomentDerivative(), IKGoal::NumDims(), IKGoal::posConstraint, IKGoal::rotConstraint, and Math::Sign().

void ForwardDifferenceAngularVelocity ( const Matrix3 R0,
const Matrix3 R1,
Real  h,
Vector3 z 
)

Computes the angular velocity z at time 0, given current rotation and rotation at next time step.

R0=R(0), R1=R(h), h is the time step.

References Math3D::Matrix3::getCrossProduct().

void GetDefaultIKDofs ( const RobotKinematics3D robot,
const std::vector< IKGoal > &  ik,
ArrayMapping m 
)

Computes the ArrayMapping that only includes ancestor links of the goals in ik. This reduces the size of a RobotIKFunction's Jacobian matrix

References ArrayUtils::copy(), Chain::LCA(), Chain::parents, and RobotKinematics3D::qMax.

Referenced by CartesianDriveSolver::Drive(), and SolveIK().

void GetMinimalRotation ( const Vector3 x,
const Vector3 y,
Matrix3 R 
)

Calculates the rotation matrix R that rotates x onto y, minimal in the sense of minimal angle. x,y are unit vectors.

References Math::Clamp(), Math::FuzzyEquals(), Math3D::GetCanonicalBasis(), and Math::Inv().

Referenced by IKGoal::GetClosestGoalTransform(), and IKGoal::SetFromPoints().

void GetMinimalRotationToPlane ( const Vector3 x,
const Vector3 y,
Matrix3 R 
)

Calculates the rotation matrix R that rotates x to be orthogonal to y, minimal in the sense of minimal angle. x,y are unit vectors.

References Math::FuzzyEquals(), Math3D::Vector3::getOrthogonalBasis(), and Math::Inv().

void GetPassiveChainDOFs ( const RobotKinematics3D robot,
const IKGoal ikGoal,
ArrayMapping passiveDofs 
)

Computes the ArrayMapping that contains a nonredundant subset of links terminating in the link of ikGoal.

References GetPassiveChainDOFs(), IKGoal::link, IKGoal::posConstraint, and IKGoal::rotConstraint.

Referenced by GetPassiveChainDOFs(), and SolvePassiveChainIK().

void GetPassiveChainDOFs ( const RobotKinematics3D robot,
int  link,
int  numTerms,
ArrayMapping passiveDofs 
)

The same as above, but the goal is specified only by the link index and the number of operational space dof's.

References Chain::parents, and RobotKinematics3D::qMax.

template<class Rot >
void GetRotationAboutLocalPoint ( const Vector3 localpt,
const Vector3 pt,
const Rot &  r,
RigidTransform T 
)

Returns the RigidTransform that moves localpt onto pt, with the given rotation r.

Referenced by IKGoal::SetFromPoints().

bool GetRotationCenter ( const RigidTransform T,
Vector3 p 
)

Calculates the fixed point of the RigidTransform T. Returns false if T contains no rotation.

bool IntersectGoals ( const IKGoal a,
const IKGoal b,
IKGoal c,
Real  tolerance = 1e-5 
)

Constructs a new goal c from the intersection of two goals a and b which are required to have the same link and destLink. False is returned if a and b are incompatible.

References IKGoal::destLink, IKGoal::direction, IKGoal::endPosition, IKGoal::endRotation, Math::FuzzyZero(), IKGoal::GetError(), IntersectGoals(), IKGoal::link, IKGoal::localAxis, IKGoal::localPosition, IKGoal::NumDims(), IKGoal::posConstraint, IKGoal::rotConstraint, and IKGoal::SetFromPoints().

Referenced by AddGoalNonredundant(), and IntersectGoals().

bool IsReachableGoal ( const RobotKinematics3D ,
const IKGoal a,
const IKGoal b 
)

Returns false if the goals a and b cannot be mutually reached because of the length of the robot links. Joint limits do not factor into this computation.

References IsReachableGoal(), and IKGoal::link.

Referenced by IsReachableGoal().

bool IsReachableGoal ( const IKGoal a,
const IKGoal b,
Real  jointDistance 
)

Returns false if the goals a and b cannot be mutually reached because of the length of the robot links, whose maximum distance is given as jointDistance.

References BallCircleCollision(), CircleCircleClosestPoints(), IKGoal::destLink, IKGoal::endPosition, IKGoal::endRotation, IKGoal::GetError(), IsReachableGoal(), IKGoal::link, IKGoal::localAxis, IKGoal::localPosition, IKGoal::NumDims(), IKGoal::posConstraint, and IKGoal::rotConstraint.

Real MatrixAbsoluteAngle ( const Matrix3 R)

Returns the absolute angle of the rotation R.

Calculated as Acos((tr(R)-1)/2)

References Math::IsFinite().

Real MatrixAngleAboutAxis ( const Matrix3 R,
const Vector3 a 
)

Returns the angle that R makes about the axis a.

If R is not a rotation about the axis, returns the angle theta such that the angle between R and R(a,theta) is minimized.

Real MatrixAngleDerivative ( const Matrix3 R,
const Vector3 z 
)

Derivative dtheta/dz, the change in the absolute angle of rotation R as it rotates about the axis z.

The angle of a rotation R is Acos((tr(R)-1)/2), and the derivative is calculated as -tr([z]*R)/2sin(theta).

References Math::FuzzyEquals(), and Math::IsFinite().

void MatrixDerivative ( const Matrix3 R,
const Vector3 z,
Matrix3 dR 
)

Derivative dR of the rotation matrix R of a frame as it rotates about the axis z.

The current rotation is given as R. The derivative is calculated with the formula dR = [z]*R.

Referenced by RobotKinematics3D::GetWorldRotationDeriv(), and MomentDerivative().

void MatrixDerivative2 ( const Matrix3 R,
const Vector3 w,
const Vector3 a,
Matrix3 ddR 
)

Second derivative ddR of the rotation matrix R of a frame as it rotates with velocity w, acceleration a about the axis z.

The current rotation is given as R. The 2nd derivative is calculated with the formula ddR = ([a]+[w]^2)*R.

References Math3D::Matrix3::setCrossProduct().

void MomentDerivative ( const Matrix3 R,
const Vector3 z,
Vector3 dm 
)

Same as above, but calculates the derivative of the moment representation of R (if R = e^[m], calculates dm/dz)

References Math::FuzzyEquals(), Math::IsFinite(), MatrixDerivative(), Math::Sinc(), and Math::Sinc_Dx().

void QuaternionDerivative ( const Matrix3 R,
const Vector3 z,
Quaternion &  dq 
)

Calculates the derivative of the quaternion representation of R as it rotates around the axis z.

bool SolveIK ( RobotIKFunction f,
Real  tolerance,
int &  iters,
int  verbose = 1 
)

Helper function that tries to solve the given ik function using the Newton-Raphson solver. Returns true if successful

Referenced by SolveIK(), and SolvePassiveChainIK().

bool SolveIK ( RobotKinematics3D ,
const std::vector< IKGoal > &  problem,
const std::vector< int > &  activeDofs,
Real  tolerance,
int &  iters,
int  verbose = 1 
)

Same as above, constructing the ik function from an IK problem and a list of active dofs

References SolveIK().

bool SolvePassiveChainIK ( RobotKinematics3D ,
const IKGoal goal,
Real  tolerance,
int &  iters,
int  verbose = 0 
)

Same as above, constructing the ik function from a single IK goal and only using the required number of degrees of freedom to solve for the given goal.

References GetPassiveChainDOFs(), and SolveIK().