KrisLibrary  1.0.0
IKFunctions.h
Go to the documentation of this file.
1 #ifndef ROBOTICS_IK_FUNCTIONS_H
2 #define ROBOTICS_IK_FUNCTIONS_H
3 
4 #include "RobotKinematics3D.h"
5 #include "IK.h"
7 #include <KrisLibrary/optimization/Newton.h>
8 #include <KrisLibrary/utils/ArrayMapping.h>
9 #include <KrisLibrary/utils/DirtyData.h>
10 
19 
33 struct RobotIKFunction : public CompositeVectorFieldFunction
34 {
36  ~RobotIKFunction();
37 
38  void UseIK(const std::vector<IKGoal>& g);
39  void UseIK(const IKGoal& g);
40  void UseCOM(const Vector2& comGoal);
41  void Clear();
42 
43  void SetState(const Vector& x) const;
44  void GetState(Vector& x) const;
45  virtual void PreEval(const Vector& x);
46 
47  RobotKinematics3D& robot;
48 
49  ArrayMapping activeDofs;
50  //vector<Real> scaleDofs; TODO? enable scaling of dofs
51 };
52 
68 {
69  RobotIKSolver(RobotIKFunction& function);
70  void UseJointLimits(Real revJointThreshold = Inf);
71  void UseJointLimits(const Vector& qmin,const Vector& qmax);
72  void UseBiasConfiguration(const Vector& qdesired);
73  void ClearJointLimits();
74  void RobotToState();
75  void StateToRobot();
76  bool Solve(Real tolerance,int& iters);
77  void PrintStats();
78 
80  RobotIKFunction& function;
81  RobotKinematics3D& robot;
82 };
83 
86 void GetDefaultIKDofs(const RobotKinematics3D& robot,const std::vector<IKGoal>& ik,ArrayMapping& m);
87 
90 void GetPassiveChainDOFs(const RobotKinematics3D& robot,const IKGoal& ikGoal,ArrayMapping& passiveDofs);
91 
94 void GetPassiveChainDOFs(const RobotKinematics3D& robot,int link,int numTerms,ArrayMapping& passiveDofs);
95 
98 bool SolveIK(RobotIKFunction& f,
99  Real tolerance,int& iters,int verbose=1);
100 
102 bool SolveIK(RobotKinematics3D&,const std::vector<IKGoal>& problem,
103  Real tolerance,int& iters,int verbose=1);
104 
107 bool SolveIK(RobotKinematics3D&,const std::vector<IKGoal>& problem,
108  const std::vector<int>& activeDofs,
109  Real tolerance,int& iters,int verbose=1);
110 
115  Real tolerance,int& iters,int verbose=0);
116 
118 void EvalIKError(const IKGoal& g,const RigidTransform& T,Real* poserr,Real* orierr);
119 void EvalIKError(const IKGoal& g,const RigidTransform& T,Vector& err);
120 Real RobotIKError(const RobotKinematics3D& robot,const IKGoal& goal);
121 
124 void EvalIKGoalDeriv(const IKGoal& g,const RigidTransform& T,const Vector3& dw,const Vector3& dv,Vector& derr);
125 
129 bool IntersectGoals(const IKGoal& a,const IKGoal& b,IKGoal& c,Real tolerance=1e-5);
133 bool AddGoalNonredundant(const IKGoal& goal,std::vector<IKGoal>& goalSet,Real tolerance=1e-5);
134 
135 
142 struct WorldPositionFunction : public VectorFieldFunction
143 {
144  WorldPositionFunction(RobotKinematics3D&,const Vector3& pi,int i,const ArrayMapping& activeDofs);
145  int GetDOF(int dim) const;
146  virtual std::string Label() const { return "WorldPos"; }
147  virtual int NumDimensions() const { return 3; }
148  virtual void Eval(const Vector& x, Vector& v);
149  virtual Real Eval_i(const Vector& x, int i);
150  virtual void Jacobian(const Vector& x, Matrix& J);
151  //virtual void Jacobian_i(const Vector& x, int i, Vector& Ji);
152  //virtual void Hessian_i(const Vector& x,int i,Matrix& Hi);
153 
154  RobotKinematics3D& robot;
155  Vector3 ploc;
156  int link;
157  const ArrayMapping& activeDofs;
158 };
159 
166 struct IKGoalFunction : public VectorFieldFunction
167 {
168  IKGoalFunction(RobotKinematics3D&,const IKGoal&,const ArrayMapping& activeDofs);
169  int GetDOF(int dim) const;
170  virtual std::string Label() const;
171  virtual std::string Label(int i) const;
172  virtual int NumDimensions() const;
173  virtual void PreEval(const Vector& x);
174  virtual void Eval(const Vector& x, Vector& r);
175  virtual Real Eval_i(const Vector& x, int i);
176  virtual void Jacobian(const Vector& x, Matrix& J);
177  virtual void Jacobian_i(const Vector& x, int i, Vector& Ji);
178  virtual void Hessian_i(const Vector& x,int i,Matrix& Hi);
179 
180  void UpdateEEPos();
181  void UpdateEERot();
182 
183  RobotKinematics3D& robot;
184  const IKGoal& goal;
185  const ArrayMapping& activeDofs;
186  Real positionScale, rotationScale;
187  //position,rotation error of end effector
188  DirtyData<Vector3> eepos;
189  DirtyData<Matrix3> eerot;
191 };
192 
199 struct RobotCOMFunction : public VectorFieldFunction
200 {
201  RobotCOMFunction(RobotKinematics3D&,const Vector2& com,const ArrayMapping& activeDofs);
202  int GetDOF(int dim) const;
203  virtual std::string Label() const;
204  virtual std::string Label(int i) const;
205  virtual int NumDimensions() const { return 2; }
206  virtual void PreEval(const Vector& x);
207  virtual void Eval(const Vector& x, Vector& r);
208  virtual Real Eval_i(const Vector& x, int i);
209  virtual void Jacobian(const Vector& x, Matrix& J);
210  virtual void Jacobian_i(const Vector& x, int i, Vector& Ji);
211  virtual void Hessian_i(const Vector& x,int i,Matrix& Hi);
212 
213  RobotKinematics3D& robot;
214  Vector2 comGoal;
215  const ArrayMapping& activeDofs;
216  Real comScale;
217  //temp
218  Real mtotal;
221 };
222 
226 bool IsReachableGoal(const RobotKinematics3D&,const IKGoal& a,const IKGoal& b);
227 
231 bool IsReachableGoal(const IKGoal& a,const IKGoal& b,Real jointDistance);
232 
235 #endif
An invertible mapping from indices in [0,imax), which operates in two modes: offset mode adds an offs...
Definition: ArrayMapping.h:20
void GetDefaultIKDofs(const RobotKinematics3D &robot, const std::vector< IKGoal > &ik, ArrayMapping &m)
Definition: IKFunctions.cpp:868
A 3D vector class.
Definition: math3d/primitives.h:136
void EvalIKGoalDeriv(const IKGoal &g, const RigidTransform &T, const Vector3 &dw, const Vector3 &dv, Vector &derr)
Definition: IKFunctions.cpp:34
bool IntersectGoals(const IKGoal &a, const IKGoal &b, IKGoal &c, Real tolerance=1e-5)
Definition: IKFunctions.cpp:1135
bool IsReachableGoal(const RobotKinematics3D &, const IKGoal &a, const IKGoal &b)
Definition: IKFunctions.cpp:981
A globally convergent Newton&#39;s method for multidimensional root solving. Solves for func(x)=0...
Definition: Newton.h:24
A vector field function class defining C(q)=0 for IK.
Definition: IKFunctions.h:33
Function class that returns the world-space position of a point on the robot.
Definition: IKFunctions.h:142
Various basic function classes.
A rigid-body transformation.
Definition: math3d/primitives.h:820
Function class that returns the position/rotation errors associated with a given IKGoal.
Definition: IKFunctions.h:166
bool SolvePassiveChainIK(RobotKinematics3D &, const IKGoal &goal, Real tolerance, int &iters, int verbose=0)
Definition: IKFunctions.cpp:970
A Newton-Raphson robot IK solver.
Definition: IKFunctions.h:67
A structure defining a link&#39;s desired configuration for IK.
Definition: IK.h:14
void EvalIKError(const IKGoal &g, const RigidTransform &T, Real *poserr, Real *orierr)
For the goal link&#39;s transformation T, computes the error of the IK goal g.
Definition: IKFunctions.cpp:17
bool AddGoalNonredundant(const IKGoal &goal, std::vector< IKGoal > &goalSet, Real tolerance=1e-5)
Definition: IKFunctions.cpp:1320
Function class that measures the difference between the robot&#39;s center of mass and a desired COM...
Definition: IKFunctions.h:199
A 2D vector class.
Definition: math3d/primitives.h:41
Class defining kinematics of a robot, and commonly used functions.
Definition: RobotKinematics3D.h:33
bool SolveIK(RobotIKFunction &f, Real tolerance, int &iters, int verbose=1)
Definition: IKFunctions.cpp:923
void GetPassiveChainDOFs(const RobotKinematics3D &robot, const IKGoal &ikGoal, ArrayMapping &passiveDofs)
Definition: IKFunctions.cpp:903