rw::math::Transform3D<double> cubic_spline(rw::math::Transform3D<double> &P_s, rw::math::Transform3D<double> &P_f, rw::math::Transform3D<double> &V_s, rw::math::Transform3D<double> &V_f, double t, double t_s, double t_f){
rw::math::Vector3D<double> pos = C(P_s.P(), P_f.P(), V_s.P(), V_f.P(), t, t_s, t_f);
rw::math::Rotation3D<double> rot = P_s.R(); //C< rw::math::Rotation3D<double> >(P_s.R(), P_f.R(), V_s.R(), V_f.R(), t, t_s, t_f);
rw::math::Transform3D<double> res(pos, rot);
return res;
}
std::vector<rw::math::Transform3D<>> pathPlanning::invKin_LargeDisplacement(rw::math::Transform3D<> t_tool_base_desired)
{
rw::math::Transform3D<> t_tool_base_current = this->device.get()->baseTend(this->state);
// Equidistant interpolation:
// dividing path between current and desired into small i_desired
// P_tool_base_desired-i = p_tool_base_current + i/m (P_base_tool_desired - p_tool_base_current)
// M - Eq: 4.25 in robotics book.. To complex to write myself!
// From M we need to define θV (desired equivalent angle axis), and tesselation = 0.01
double tesselation = 0.05;
double ws = 0.1;
// May have to be changed... text book defintion - "where W S denotes some (possibly rather crude) estimate of the size of the
// positional part of the workspace that the robot can reach"
//rotational change = t_tool_base_desired * t_tool_current.R.Transpose()
Eigen::Matrix3d rotational_change;
rotational_change = t_tool_base_desired.R().e() * t_tool_base_current.R().e().transpose();
//cout << rotational_change << endl;
//find Wrot
Eigen::Vector3d W_rot;
W_rot << 1 ,1 ,1 ;
double normalized_axis_vector_0 = rotational_change(2,1) - rotational_change(1,2);
double normalized_axis_vector_1 = rotational_change(0,2) - rotational_change(2,0);
double normalized_axis_vector_2 = rotational_change(1,0) - rotational_change(0,1);
double angle_of_rotation = acos((rotational_change.trace() - 1)/2);
double normalized_axis = 1/(2*sin(angle_of_rotation));
//Check for singularities?
if(angle_of_rotation == 0)
{
// eq: 4.19
cout << "Due to singularity use this formula" << endl;
W_rot(0) = 0.5*normalized_axis_vector_0;
W_rot(1) = 0.5*normalized_axis_vector_1;
W_rot(2) = 0.5*normalized_axis_vector_2;
}
else if(angle_of_rotation == M_PI)
{
//A bit more invovled.. Basically eq:4.20
// Bit unsure what is meant by ||(R_32 - R_23), (R_13 - R_31)||
cout << "Angle of rotation is M_PI, NOT implemented yet!" << endl;
Eigen::Vector3d normalized_axis_vector;
normalized_axis_vector << normalized_axis_vector_0 , normalized_axis_vector_1 , normalized_axis_vector_2;
angle_of_rotation = M_PI - (normalized_axis_vector.norm())/2;
//How to normalized axis.. the alpha part is tricky?
}
else
{
// eq: 4.18
W_rot(0) = angle_of_rotation*normalized_axis*normalized_axis_vector_0;
W_rot(1) = angle_of_rotation*normalized_axis*normalized_axis_vector_1;
W_rot(2) = angle_of_rotation*normalized_axis*normalized_axis_vector_2;
}
//equivalent angle axis found as Wrot.
double normed_positional_change = (t_tool_base_desired.P().e() - t_tool_base_current.P().e()).norm()/ws;
double normed_angle_axis_rotation = W_rot.norm()/M_PI;
double M = trunc(((1/tesselation)*(normed_positional_change + normed_angle_axis_rotation))+1);
std::vector<rw::math::Transform3D<>> t_tool_base_desired_i;
std::ofstream outfile_p;
std::ofstream outfile_r;
outfile_p.open("interpolate_p.txt", std::ios_base::app);
outfile_r.open("interpolate_r.txt", std::ios_base::app);
for(double i = 1; i <= M ; i += 1 )
{
rw::math::Vector3D<> position_desired_i = t_tool_base_current.P() + (i/M)*(t_tool_base_desired.P()-t_tool_base_current.P());
outfile_p << position_desired_i.e()[0] << "," << position_desired_i.e()[1] << "," << position_desired_i.e()[2] << endl;
Eigen::Vector3d v;
v << normalized_axis_vector_0 , normalized_axis_vector_1 , normalized_axis_vector_2;
// cout << normalized_axis_vector_0 << normalized_axis_vector_1 << normalized_axis_vector_2 << endl;
v = normalized_axis * v; //* (i/M);
angle_of_rotation = angle_of_rotation*(i/M);
Eigen::Matrix3d R_eaa;
double _0_0 = pow(v(0),2.0)*(1-cos(angle_of_rotation))+cos(angle_of_rotation);
double _0_1 = v(0)*v(1)*(1-cos(angle_of_rotation))-v(2)*sin(angle_of_rotation);
double _0_2 = v(0)*v(2)*(1-cos(angle_of_rotation))+v(1)*sin(angle_of_rotation);
double _1_0 = v(0)*v(1)*(1-cos(angle_of_rotation))+v(2)*sin(angle_of_rotation);
double _1_1 = pow(v(1),2.0)*(1-cos(angle_of_rotation))+cos(angle_of_rotation);
double _1_2 = v(1)*v(2)*(1-cos(angle_of_rotation))*v(0)*sin(angle_of_rotation);
double _2_0 = v(0)*v(2)*(1-cos(angle_of_rotation))-v(1)*sin(angle_of_rotation);
double _2_1 = v(1)*v(2)*(1-cos(angle_of_rotation))+v(0)*sin(angle_of_rotation);
double _2_2 = pow(v(2),2)*(1-cos(angle_of_rotation))+ cos(angle_of_rotation);
R_eaa << _0_0 , _0_1 , _0_2 ,
_1_0 , _1_1 , _1_2 ,
_2_0 , _2_1 , _2_2;
Eigen::Matrix3d rotation_desired_i_eigen = R_eaa*t_tool_base_current.R().e();
rw::math::Rotation3D<> rotation_desired_i(rotation_desired_i_eigen);
outfile_r << rotation_desired_i.e() << endl << endl;
rw::math::Transform3D<> transform_i(position_desired_i,rotation_desired_i);
t_tool_base_desired_i.push_back(transform_i);
}
return t_tool_base_desired_i;
}
