void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) {
Vector v2;
// Wrench
Wrench w(Vector(7,-1,3),Vector(2,-3,3));
Twist t(Vector(6,3,5),Vector(4,-2,7));
// Rotation
Rotation R;
Rotation R2;
double ra;
double rb;
double rc;
R = Rotation::RPY(a,b,c);
CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitX()),1.0,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitY()),1.0,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitZ(),R.UnitZ()),1.0,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitY()),0.0,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitZ()),0.0,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitZ()),0.0,epsilon);
R2=R;
CPPUNIT_ASSERT_EQUAL(R,R2);
CPPUNIT_ASSERT_DOUBLES_EQUAL((R*v).Norm(),v.Norm(),epsilon);
CPPUNIT_ASSERT_EQUAL(R.Inverse(R*v),v);
CPPUNIT_ASSERT_EQUAL(R.Inverse(R*t),t);
CPPUNIT_ASSERT_EQUAL(R.Inverse(R*w),w);
CPPUNIT_ASSERT_EQUAL(R*R.Inverse(v),v);
CPPUNIT_ASSERT_EQUAL(R*Rotation::Identity(),R);
CPPUNIT_ASSERT_EQUAL(Rotation::Identity()*R,R);
CPPUNIT_ASSERT_EQUAL(R*(R*(R*v)),(R*R*R)*v);
CPPUNIT_ASSERT_EQUAL(R*(R*(R*t)),(R*R*R)*t);
CPPUNIT_ASSERT_EQUAL(R*(R*(R*w)),(R*R*R)*w);
CPPUNIT_ASSERT_EQUAL(R*R.Inverse(),Rotation::Identity());
CPPUNIT_ASSERT_EQUAL(R.Inverse()*R,Rotation::Identity());
CPPUNIT_ASSERT_EQUAL(R.Inverse()*v,R.Inverse(v));
R.GetRPY(ra,rb,rc);
CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
R = Rotation::EulerZYX(a,b,c);
R.GetEulerZYX(ra,rb,rc);
CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
R = Rotation::EulerZYZ(a,b,c);
R.GetEulerZYZ(ra,rb,rc);
CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
double angle= R.GetRotAngle(v2);
R2=Rotation::Rot2(v2,angle);
CPPUNIT_ASSERT_EQUAL(R2,R);
R2=Rotation::Rot(v2*1E20,angle);
CPPUNIT_ASSERT_EQUAL(R,R2);
v2=Vector(6,2,4);
CPPUNIT_ASSERT_DOUBLES_EQUAL((v2).Norm(),::sqrt(dot(v2,v2)),epsilon);
}
void ChainTask::RPY_angles(Rotation rot)
{
rot.GetRPY(chi_f_spatula(0),chi_f_spatula(1),chi_f_spatula(2));
geometry_msgs::PoseStamped pp;
Frame ff = Frame(Rotation::RPY(desired_values[3],desired_values[4],desired_values[5]));
// stupid hack for rviz (running locally, wtf?!)
pp.header.stamp = ros::Time::now() - ros::Duration(0.1);
pp.header.frame_id = "/spatula";
tf::PoseKDLToMsg(ff, pp.pose);
ros_desired_pose_port.write(pp);
// compute Jacobian matrix
//reference frame is spatula, reference point is end of spatula chain
jacsolver_spatula->JntToJac(chi_f_spatula_init, Jf_spatula);
}
KDL::JntArray cyl_irpy_chain(const KDL::Frame& frame)
{
static KDL::ChainFkSolverPos_recursive* fksolver_baker=0;
if(fksolver_baker == 0)
{
KDL::Chain chain_baker;
chain_baker.addSegment(Segment(Joint(Joint::RotZ)));
chain_baker.addSegment(Segment(Joint(Joint::TransX)));
chain_baker.addSegment(Segment(Joint(Joint::TransZ),
Frame(Rotation::RotX(M_PI/2)*Rotation::RotY(-M_PI/2))));
fksolver_baker = new ChainFkSolverPos_recursive(chain_baker);
}
JntArray chi_f(6);
Vector p = frame.p;
//printf("atan2: %f\n", atan2(p.y(),p.x()));
chi_f(0)=atan2(p.y(),p.x());
chi_f(1)=sqrt(p.x()*p.x()+p.y()*p.y());
chi_f(2)=p.z();
//printf("chi_f(0): %f\n", chi_f(0));
Frame baker_pose;
JntArray chi_f_baker(3);
chi_f_baker(0) = chi_f(0);
chi_f_baker(1) = chi_f(1);
chi_f_baker(2) = chi_f(2);
fksolver_baker->JntToCart(chi_f_baker, baker_pose);
// rotations
Rotation rot = (baker_pose.Inverse()*frame).M.Inverse();
rot.GetRPY(chi_f(3),chi_f(4),chi_f(5));
return chi_f;
}
double operator()(const Rotation& rot ) const
{
double r,p,y;
rot.GetRPY(r,p,y);
return y;
}