16 enum PosConstraint { PosNone=0, PosPlanar=1, PosLinear=2, PosFixed=3 };
17 enum RotConstraint { RotNone=0, RotTwoAxis=1, RotAxis=2, RotFixed=3 };
20 inline static int NumDims(PosConstraint c) {
return (
int)c; }
21 inline static int NumDims(RotConstraint c) {
return (
int)c; }
29 void SetPlanarPosition(
const Vector3& point,
const Vector3& normal);
32 void SetLinearPosition(
const Vector3& point,
const Vector3& direction);
37 void SetFixedRotation(
const Matrix3& R);
42 void SetAxisRotation(
const Vector3& localAxis,
const Vector3& worldAxis);
53 void SetFromPoints(
const std::vector<Vector3>& localpos,
const std::vector<Vector3>& worldpos,Real degeneracyTol=Epsilon);
55 void RemovePositionAxis(
const Vector3& axis);
57 void RemoveRotationAxis(
const Vector3& axis);
67 void GetError(
const RigidTransform& Trel,Real posErr[3],Real rotErr[3])
const;
68 void GetFixedGoalRotation(
Matrix3& R)
const;
70 void GetBaseEdgeRotation(
Matrix3& R0)
const;
106 std::istream& operator >> (std::istream& in,
IKGoal& data);
107 std::ostream& operator << (std::ostream& out,
const IKGoal& data);
void SetFixedPosition(const Vector3 &pos)
Definition: IK.h:26
A 3D vector class.
Definition: math3d/primitives.h:136
Vector3 endRotation
Definition: IK.h:103
Vector3 direction
Definition: IK.h:94
Vector3 localAxis
If in axis-constrained rotation, the local rot axis.
Definition: IK.h:99
Class declarations for useful 3D math types.
int destLink
Desintation link index (-1 means world frame)
Definition: IK.h:84
void SetFixedRotation(const Vector3 &mR)
Contrains the link to have a fixed rotation moment.
Definition: IK.h:39
RotConstraint rotConstraint
Rotation terms:
Definition: IK.h:97
Vector3 endPosition
The desired end effector position.
Definition: IK.h:91
A structure defining a link's desired configuration for IK.
Definition: IK.h:14
void SetFreeRotation()
Removes the rotation constraint.
Definition: IK.h:44
PosConstraint posConstraint
Position terms:
Definition: IK.h:87
Contains all the definitions in the Math3D package.
Definition: AnyGeometry.h:12
Vector3 localPosition
End effector position in the local frame.
Definition: IK.h:89
A 3x3 matrix class.
Definition: math3d/primitives.h:469
void SetFreePosition()
Removes the position constraint.
Definition: IK.h:34
static int NumDims(PosConstraint c)
Returns the number of operational space dof's of the given constraint.
Definition: IK.h:20
int link
Robot link index.
Definition: IK.h:82