trajectory_interpolator.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
 
#include <math.h>
#include <vector>
 
#include <cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h>
#include <cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h>
 
bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_array,
                                                 const cob_cartesian_controller::CartesianActionStruct as)
{
    this->trajectory_profile_generator_.reset(TrajectoryProfileBuilder::createProfile(as));
 
    pose_array.header.stamp = ros::Time::now();
    pose_array.header.frame_id = root_frame_;
 
    tf::Quaternion q_start, q_end;
 
    std::vector<double> linear_path, angular_path, path;
    std::vector<double> path_matrix[2];
    geometry_msgs::Pose pose;
 
    double norm_factor;
    tf::quaternionMsgToTF(as.move_lin.start.orientation, q_start);
    tf::quaternionMsgToTF(as.move_lin.end.orientation, q_end);
 
    double Se_lin = sqrt(pow((as.move_lin.end.position.x - as.move_lin.start.position.x), 2) +
                         pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) +
                         pow((as.move_lin.end.position.z - as.move_lin.start.position.z), 2));
 
    double Se_rot = q_start.angleShortestPath(q_end);
 
    if (!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot))
    {
        return false;
    }
 
    linear_path  = path_matrix[0];
    angular_path = path_matrix[1];
 
    if (fabs(linear_path.back()) > fabs(angular_path.back()))
    {
        path = linear_path;
    }
    else
    {
        path = angular_path;
    }
    norm_factor = 1/path.back();
 
    // Interpolate the linear path
    for (unsigned int i = 0; i < linear_path.size(); i++)
    {
        if (linear_path.back() == 0)
        {
            pose.position.x = as.move_lin.start.position.x;
            pose.position.y = as.move_lin.start.position.y;
            pose.position.z = as.move_lin.start.position.z;
        }
        else
        {
            pose.position.x = as.move_lin.start.position.x + linear_path.at(i) * (as.move_lin.end.position.x - as.move_lin.start.position.x) / linear_path.back();
            pose.position.y = as.move_lin.start.position.y + linear_path.at(i) * (as.move_lin.end.position.y - as.move_lin.start.position.y) / linear_path.back();
            pose.position.z = as.move_lin.start.position.z + linear_path.at(i) * (as.move_lin.end.position.z - as.move_lin.start.position.z) / linear_path.back();
        }
 
        tf::quaternionTFToMsg(q_start.slerp(q_end, path.at(i) * norm_factor), pose.orientation);
        pose_array.poses.push_back(pose);
    }
    return true;
}
 
bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pose_array,
                                                   const cob_cartesian_controller::CartesianActionStruct as)
{
     pose_array.header.stamp = ros::Time::now();
     pose_array.header.frame_id = root_frame_;
 
     tf::Quaternion q;
     tf::Transform C, P, T;
 
     std::vector<double> path_array;
     std::vector<double> path_matrix[2];
 
     geometry_msgs::Pose pose;
 
     double Se = as.move_circ.end_angle - as.move_circ.start_angle;
 
     bool forward;
 
     // Needed for the circle direction!
     if (Se < 0)
     {
         forward = false;
     }
     else
     {
         forward = true;
     }
     Se = std::fabs(Se);
 
     // Calculates the Path with RAMP or SINOID profile
     if (!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0))
     {
         return false;
     }
 
     path_array = path_matrix[0];
     // Define Center Pose
     C.setOrigin(tf::Vector3(as.move_circ.pose_center.position.x, as.move_circ.pose_center.position.y, as.move_circ.pose_center.position.z));
     tf::quaternionMsgToTF(as.move_circ.pose_center.orientation, q);
     C.setRotation(q);
 
     // Interpolate the circular path
     for (unsigned int i = 0; i < path_array.size(); i++)
     {
         // Rotate T
         T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius));
 
         if (forward)
         {
             T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius));
             q.setRPY(0, -path_array.at(i), 0);
         }
         else
         {
             T.setOrigin(tf::Vector3(cos(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius, 0, sin(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius));
             q.setRPY(0, path_array.at(i), 0);
         }
 
         T.setRotation(q);
 
         // Calculate TCP Position
         P = C * T;
 
         tf::pointTFToMsg(P.getOrigin(), pose.position);
         tf::quaternionTFToMsg(P.getRotation(), pose.orientation);
 
         pose_array.poses.push_back(pose);
     }
 
    return true;
}