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; }