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));
}
Example #2
0
void IKGoal::SetFixedRotation(const Matrix3& R)
{
  rotConstraint = RotFixed;
  MomentRotation mR;
  mR.setMatrix(R);
  endRotation = mR;
}
Example #3
0
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;
}
Example #4
0
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;
}
Example #5
0
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");
  }
}
Example #6
0
  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));
  }
Example #7
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;
  }
}
Example #8
0
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;
}