示例#1
0
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;
}
示例#2
0
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;

}