KrisLibrary  1.0.0
IK.h
1 #ifndef ROBOTICS_IK_H
2 #define ROBOTICS_IK_H
3 
5 #include <vector>
6 using namespace Math3D;
7 
14 struct IKGoal
15 {
16  enum PosConstraint { PosNone=0, PosPlanar=1, PosLinear=2, PosFixed=3 };
17  enum RotConstraint { RotNone=0, RotTwoAxis=1, RotAxis=2, RotFixed=3 };
18 
20  inline static int NumDims(PosConstraint c) { return (int)c; }
21  inline static int NumDims(RotConstraint c) { return (int)c; }
22 
23  IKGoal();
26  void SetFixedPosition(const Vector3& pos) { posConstraint=PosFixed; endPosition=pos; }
29  void SetPlanarPosition(const Vector3& point,const Vector3& normal);
32  void SetLinearPosition(const Vector3& point,const Vector3& direction);
34  void SetFreePosition() { posConstraint=PosNone; }
35 
37  void SetFixedRotation(const Matrix3& R);
39  void SetFixedRotation(const Vector3& mR) { rotConstraint=RotFixed; endRotation=mR; }
42  void SetAxisRotation(const Vector3& localAxis,const Vector3& worldAxis);
44  void SetFreeRotation() { rotConstraint=RotNone; }
45 
47  void SetFixedTransform(const RigidTransform& T);
48 
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);
59  void RemoveRotationAxis(const Vector3& p,const Vector3& axis);
61  void Transform(const RigidTransform& T);
63  void TransformLocal(const RigidTransform& T);
64 
67  void GetError(const RigidTransform& Trel,Real posErr[3],Real rotErr[3]) const;
68  void GetFixedGoalRotation(Matrix3& R) const;
69  void GetFixedGoalTransform(RigidTransform& T) const;
70  void GetBaseEdgeRotation(Matrix3& R0) const;
71  void GetEdgeGoalTransform(Real theta,RigidTransform& T) const;
72 
75  void GetClosestGoalTransform(const RigidTransform& T0,RigidTransform& T) const;
76 
79  void MatchGoalTransform(const RigidTransform& Trel);
80 
82  int link;
84  int destLink;
85 
87  PosConstraint posConstraint;
95 
97  RotConstraint rotConstraint;
104 };
105 
106 std::istream& operator >> (std::istream& in,IKGoal& data);
107 std::ostream& operator << (std::ostream& out,const IKGoal& data);
108 
109 #endif
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 rigid-body transformation.
Definition: math3d/primitives.h:820
A structure defining a link&#39;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&#39;s of the given constraint.
Definition: IK.h:20
int link
Robot link index.
Definition: IK.h:82