//! old task angle position controller
void ChainTask::doControl()
{
Rotation desired = Rotation::RPY(desired_values[3],desired_values[4],desired_values[5]);
Rotation measured = Rotation::RPY(chi_f_spatula(0),chi_f_spatula(1),chi_f_spatula(2));
Vector rot = diff(desired,measured);
rot = measured.Inverse()*rot;
for(unsigned int i=0;i<NC;i++) {
if(i<N_CHIF_BAKER || new_rotation)
ydot(i)=feedback_gain[i]*(desired_values[i] - chi_f(i));
else
ydot(i)=feedback_gain[i]*rot(i-N_CHIF_BAKER);
}
}
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 JacobianTest::TestChangeBase(){
//Create a random jacobian
Jacobian j1(5);
j1.data.setRandom();
//Create a random rotation
Rotation r;
random(r);
Jacobian j2(5);
CPPUNIT_ASSERT(changeBase(j1,r,j2));
CPPUNIT_ASSERT(j1!=j2);
Jacobian j3(4);
CPPUNIT_ASSERT(!changeBase(j1,r,j3));
j3.resize(5);
CPPUNIT_ASSERT(changeBase(j2,r.Inverse(),j3));
CPPUNIT_ASSERT_EQUAL(j1,j3);
}
//! new range-controller.
//! this controller accepts ranges of accepted positions and
//! lowers the weight to zero when inside that range.
void ChainTask::doControl_ranges()
{
/// HACK FOR THE RPY ANGLES, PART 1
double middle[3], margin[3];
for(int i=0; i < 3; i++)
{
double lo = ros_constraint_command.pos_lo[i+3];
double hi = ros_constraint_command.pos_hi[i+3];
middle[i] = (hi + lo)/2.0;
margin[i] = (hi - lo)/2.0;
}
Rotation desired = Rotation::RPY(middle[0], middle[1], middle[2]);
Rotation measured = Rotation::RPY(chi_f_spatula(0), chi_f_spatula(1), chi_f_spatula(2));
Vector rot = diff(desired, measured);
rot = measured.Inverse()*rot;
/// ///
double s = 0.05;
for(int i=0; i < 6; i++)
{
if(ros_constraint_command.weight[i] == 0.0)
{
ydot(i) = 0.0;
continue;
}
double value = chi_f(i);
double lo = ros_constraint_command.pos_lo[i];
double hi = ros_constraint_command.pos_hi[i];
if(i >= 3 && !new_rotation) /// HACK FOR THE RPY ANGLES, PART 2
{
lo = value + rot(i-3) - margin[i-3];
hi = value + rot(i-3) + margin[i-3];
} /// ///
// adjust margin if range is too small
double ss = (hi - lo < 2*s) ? (hi - lo) / 2 : s;
if(value > hi - ss)
{
ydot(i) = feedback_gain[i]*(hi - ss - value);
desired_values[i] = hi - ss;
}
else if(value < lo + ss)
{
ydot(i) = feedback_gain[i]*(lo + ss - value);
desired_values[i] = lo + ss;
}
else
{
ydot(i) = 0.0;
desired_values[i] = value;
}
if(value > hi || value < lo)
{
weights[i] = 1.0;
}
else
{
double w_lo = (1/s)*(-hi + value)+1;
double w_hi = (1/s)*( lo - value)+1;
w_lo = (w_lo > 0.0) ? w_lo : 0.0;
w_hi = (w_hi > 0.0) ? w_hi : 0.0;
weights[i] = (w_lo > w_hi) ? w_lo : w_hi;
}
}
}
Rotation Inverse(const Rotation& rot){
return rot.Inverse();
}
Rotation operator()(const Rotation& rot ) const
{
return rot.Inverse();
}
