void IKSolver::getResidual(std::vector<double>& out)
{
int size = 0;
for(size_t i=0;i<objectives.size();i++) {
int m=IKGoal::NumDims(objectives[i].goal.posConstraint);
int n=IKGoal::NumDims(objectives[i].goal.rotConstraint);
size += m + n;
}
out.resize(size);
size = 0;
for(size_t i=0;i<objectives.size();i++) {
const IKGoal& goal = objectives[i].goal;
int m=IKGoal::NumDims(goal.posConstraint);
int n=IKGoal::NumDims(goal.rotConstraint);
Real poserr[3],orierr[3];
if(goal.destLink < 0)
goal.GetError(robot.robot->links[goal.link].T_World,poserr,orierr);
else {
RigidTransform Trel;
Trel.mulInverseB(robot.robot->links[goal.link].T_World,robot.robot->links[goal.destLink].T_World);
goal.GetError(Trel,poserr,orierr);
}
for(int k=0;k<m;k++,size++)
out[size] = poserr[k];
for(int k=0;k<n;k++,size++)
out[size] = orierr[k];
}
}
Real RobotIKError(const RobotKinematics3D& robot,const IKGoal& goal)
{
int m=IKGoal::NumDims(goal.posConstraint);
int n=IKGoal::NumDims(goal.rotConstraint);
Real poserr[3],orierr[3];
if(goal.destLink < 0)
goal.GetError(robot.links[goal.link].T_World,poserr,orierr);
else {
RigidTransform Trel;
Trel.mulInverseB(robot.links[goal.link].T_World,robot.links[goal.destLink].T_World);
goal.GetError(Trel,poserr,orierr);
}
Real emax=0.0;
for(int i=0;i<m;i++)
emax = Max(emax,Abs(poserr[i]));
for(int i=0;i<n;i++)
emax = Max(emax,Abs(orierr[i]));
return emax;
}
