KrisLibrary
1.0.0
|
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. | |
Classes to define robot kinematics.
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().
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().
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().
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().
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().
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.
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.
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().
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().
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.
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().
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().
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().
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().
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().