1 #ifndef ROBOTICS_IK_FUNCTIONS_H 2 #define ROBOTICS_IK_FUNCTIONS_H 7 #include <KrisLibrary/optimization/Newton.h> 8 #include <KrisLibrary/utils/ArrayMapping.h> 9 #include <KrisLibrary/utils/DirtyData.h> 38 void UseIK(
const std::vector<IKGoal>& g);
39 void UseIK(
const IKGoal& g);
40 void UseCOM(
const Vector2& comGoal);
43 void SetState(
const Vector& x)
const;
44 void GetState(Vector& x)
const;
45 virtual void PreEval(
const Vector& x);
70 void UseJointLimits(Real revJointThreshold = Inf);
71 void UseJointLimits(
const Vector& qmin,
const Vector& qmax);
72 void UseBiasConfiguration(
const Vector& qdesired);
73 void ClearJointLimits();
76 bool Solve(Real tolerance,
int& iters);
99 Real tolerance,
int& iters,
int verbose=1);
103 Real tolerance,
int& iters,
int verbose=1);
108 const std::vector<int>& activeDofs,
109 Real tolerance,
int& iters,
int verbose=1);
115 Real tolerance,
int& iters,
int verbose=0);
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);
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);
186 Real positionScale, rotationScale;
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);
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'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.
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'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'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'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