int Robot::dribbleToLocation(Eigen::Vector3d location) {
_maxVelocity = PIDController::MAX_TRANS_VEL/10;
_maxAngularVelocity = PIDController::MAX_ROT_VEL/ 10;
currentTime = 0.2;
desiredLocation = location;
initialLocation = CurrentState();
controller = PIDController();
return execute();
}
int Robot::goToLocation(int currentFrame, Eigen::Vector3d location) {
currentTime = 0.2;
desiredLocation = location;
initialLocation = CurrentState();
controller = PIDController();
_maxVelocity = PIDController::MAX_TRANS_VEL;
_maxAngularVelocity = PIDController::MAX_ROT_VEL;
return execute();
}
geometry_msgs::Twist cob_cartesian_trajectories::getTwist(double dt, Frame F_current)
{
KDL::Frame F_target;
KDL::Frame F_diff;
geometry_msgs::Twist ControllTwist;
double start_roll, start_pitch, start_yaw = 0.0;
double current_roll, current_pitch, current_yaw = 0.0;
double target_roll, target_pitch, target_yaw = 0.0;
if(!bStarted)
{
F_start = F_current;
F_start.M.GetRPY(last_rpy_angles["target_roll"], last_rpy_angles["target_pitch"], last_rpy_angles["target_yaw"]);
F_start.M.GetRPY(last_rpy_angles["current_roll"], last_rpy_angles["current_pitch"], last_rpy_angles["current_yaw"]);
bStarted = true;
}
getTargetPosition(dt, F_target);
F_start.M.GetRPY(start_roll, start_pitch, start_yaw);
F_current.M.GetRPY(current_roll, current_pitch, current_yaw);
F_target.M.GetRPY(target_roll, target_pitch, target_yaw);
//std::cout << "AngleDiff: " << (current_yaw-start_yaw) << " vs " << (soll_angle) << " error: " << (soll_angle-current_yaw-start_yaw) << "\n";
std::cout << "Target (x,y): " << F_target.p.x() << ", " << F_target.p.y() << "\n";
std::cout << "Error (x,y): " << F_target.p.x()-F_current.p.x() << ", " << F_target.p.y()-F_current.p.y() << "\n";
std::cout << "Start (x,y): " << F_start.p.x() << ", " << F_start.p.y() << "\n";
std::cout << "Current (x,y): " << F_current.p.x() << ", " << F_current.p.y() << "\n";
// calling PIDController to calculate the actuating variable and get back twist
ControllTwist = PIDController(dt, F_target, F_current);
//DEBUG
F_diff.p.x(F_target.p.x()-F_current.p.x());
F_diff.p.y(F_target.p.y()-F_current.p.y());
F_diff.p.z(F_target.p.z()-F_current.p.z());
geometry_msgs::PoseArray poses;
poses.poses.resize(3);
tf::PoseKDLToMsg(F_current, poses.poses[0]);
tf::PoseKDLToMsg(F_target, poses.poses[1]);
tf::PoseKDLToMsg(F_diff, poses.poses[2]);
debug_cart_pub_.publish(poses);
//std::cout << "Twist x: " << 0.1 * F_diff.p.x() << " y: " << 0.1 * F_diff.p.y() << "\n";
//
return ControllTwist;
}
