void SO3CSpace::SetRotation(const Math3D::Matrix3& R,Config& x)
{
MomentRotation m;
m.setMatrix(R);
x.resize(3);
m.get(x(0),x(1),x(2));
}
void IKGoal::SetFixedRotation(const Matrix3& R)
{
rotConstraint = RotFixed;
MomentRotation mR;
mR.setMatrix(R);
endRotation = mR;
}
Real IKGoalFunction::Eval_i(const Vector& x, int i)
{
if(i<IKGoal::NumDims(goal.posConstraint)) {
UpdateEEPos();
if(goal.posConstraint == IKGoal::PosFixed) {
return positionScale*eepos[i];
}
else if(goal.posConstraint == IKGoal::PosLinear) {
Vector3 xb,yb;
Vector3 d;
if(goal.destLink < 0) d=goal.direction;
else d=robot.links[goal.destLink].T_World.R*goal.direction;
GetCanonicalBasis(d,xb,yb);
if(i == 0) return positionScale*dot(eepos,xb);
else return positionScale*dot(eepos,yb);
}
else if(goal.posConstraint == IKGoal::PosPlanar) {
Vector3 d;
if(goal.destLink < 0) d=goal.direction;
else d=robot.links[goal.destLink].T_World.R*goal.direction;
return positionScale*dot(eepos,d);
}
}
else {
i-=IKGoal::NumDims(goal.posConstraint);
Assert(i<IKGoal::NumDims(goal.rotConstraint));
UpdateEERot();
if(goal.rotConstraint==IKGoal::RotFixed) {
MomentRotation em;
Assert(IsFinite(eerot));
em.setMatrix(eerot);
return rotationScale*em[i];
//return eerot[i];
}
else if(goal.rotConstraint==IKGoal::RotAxis) {
Vector3 x,y;
Vector3 d;
if(goal.destLink < 0) d=goal.endRotation;
else d=robot.links[goal.destLink].T_World.R*goal.endRotation;
GetCanonicalBasis(d,x,y);
Vector3 curAxis;
robot.links[goal.link].T_World.R.mul(goal.localAxis,curAxis);
if(i==0)
return rotationScale*dot(curAxis,x);
else
return rotationScale*dot(curAxis,y);
}
else {
printf("IK(): Invalid number of rotation terms\n");
Abort();
}
}
return Zero;
}
void LocalContactsToHold(const vector<ContactPoint>& contacts,int link,const RobotKinematics3D& robot,Hold& hold)
{
hold.link = link;
hold.contacts = contacts;
for(size_t i=0;i<contacts.size();i++) {
hold.contacts[i].x = robot.links[link].T_World*hold.contacts[i].x;
hold.contacts[i].n = robot.links[link].T_World.R*hold.contacts[i].n;
}
MomentRotation m;
m.setMatrix(robot.links[link].T_World.R);
hold.SetupIKConstraint(contacts[0].x,m);
hold.ikConstraint.destLink = -1;
}
void IKGoalFunction::Eval(const Vector& x, Vector& r)
{
Assert(r.n == NumDimensions());
UpdateEEPos();
if(goal.posConstraint == IKGoal::PosFixed) {
for(int j=0;j<3;j++) {
r(j) = positionScale*eepos[j];
}
}
else if(goal.posConstraint == IKGoal::PosLinear) {
Vector3 xb,yb;
Vector3 d;
if(goal.destLink < 0) d=goal.direction;
else d=robot.links[goal.destLink].T_World.R*goal.direction;
GetCanonicalBasis(d,xb,yb);
r(0) = positionScale*dot(eepos,xb);
r(1) = positionScale*dot(eepos,yb);
}
else if(goal.posConstraint == IKGoal::PosPlanar) {
Vector3 d;
if(goal.destLink < 0) d=goal.direction;
else d=robot.links[goal.destLink].T_World.R*goal.direction;
r(0) = positionScale*dot(eepos,d);
}
UpdateEERot();
int m=goal.posConstraint;
if(goal.rotConstraint==IKGoal::RotFixed) {
MomentRotation em;
Assert(IsFinite(eerot));
em.setMatrix(eerot);
r(m)=rotationScale*em.x;
r(m+1)=rotationScale*em.y;
r(m+2)=rotationScale*em.z;
}
else if(goal.rotConstraint==IKGoal::RotAxis) {
Vector3 x,y;
Vector3 d;
if(goal.destLink < 0) d=goal.endRotation;
else d=robot.links[goal.destLink].T_World.R*goal.endRotation;
GetCanonicalBasis(d,x,y);
Vector3 curAxis;
robot.links[goal.link].T_World.R.mul(goal.localAxis,curAxis);
r(m) = rotationScale*dot(curAxis,x);
r(m+1) = rotationScale*dot(curAxis,y);
}
else if(goal.rotConstraint==IKGoal::RotNone) { }
else {
FatalError("IK(): Invalid number of rotation terms");
}
}
virtual void Eval(Real t,Vector& v)
{
Matrix3 Ra,ARaB;
AngleAxisRotation aa;
aa.angle=t;
aa.axis=a;
aa.getMatrix(Ra);
ARaB = A*Ra*B;
MomentRotation m;
m.setMatrix(ARaB);
v.resize(3);
m.get(&v(0));
}
void IKGoal::Transform(const RigidTransform& T)
{
endPosition = T*endPosition;
direction = T.R*direction;
if(rotConstraint == IKGoal::RotFixed) {
MomentRotation m; m.set(endRotation);
Matrix3 R;
m.getMatrix(R);
R = T.R*R;
Assert(IsFinite(R));
m.setMatrix(R);
endRotation = m;
}
else if(rotConstraint == IKGoal::RotAxis) {
endRotation = T.R*endRotation;
}
}
void EvalIKGoalDeriv(const IKGoal& g,const RigidTransform& T,const Vector3& dw,const Vector3& dv,Vector& derr)
{
Vector3 pv=dv + cross(dw,g.endPosition-T.t);
if(g.posConstraint == IKGoal::PosFixed) {
pv.get(derr(0),derr(1),derr(2));
}
else if(g.posConstraint == IKGoal::PosLinear) {
Vector3 xb,yb;
GetCanonicalBasis(g.direction,xb,yb);
derr(0) = dot(pv,xb);
derr(1) = dot(pv,yb);
}
else if(g.posConstraint == IKGoal::PosPlanar) {
derr(0) = dot(pv,g.direction);
}
int m=IKGoal::NumDims(g.posConstraint);
if(g.rotConstraint==IKGoal::RotFixed) {
Matrix3 TgoalInv,Trel;
MomentRotation endinv;
endinv.setNegative(g.endRotation);
endinv.getMatrix(TgoalInv);
Trel.mul(T.R,TgoalInv);
Vector3 dr;
MomentDerivative(Trel,dw,dr);
dr.get(derr(0),derr(1),derr(2));
}
else if(g.rotConstraint==IKGoal::RotAxis) {
Vector3 x,y;
GetCanonicalBasis(g.endRotation,x,y);
Vector3 curAxis;
T.R.mul(g.localAxis,curAxis);
derr(m) = dot(cross(dw,curAxis),x);
derr(m+1) = dot(cross(dw,curAxis),y);
}
else if(g.rotConstraint==IKGoal::RotNone) { }
else {
FatalError("EvalIKGoalDeriv(): Invalid number of rotation terms");
}
}
void RobotKinematics3D::GetWorldRotation_Moment(int i, Vector3& theta) const
{
MomentRotation rot;
rot.setMatrix(links[i].T_World.R);
theta=rot;
}
