Skip to content
Snippets Groups Projects
Commit 58c81c7e authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

integration of test-trajectory, integration of velocity-adaption, todo: refactor/doc

parent 8d258e75
No related branches found
No related tags found
No related merge requests found
//
// Created by sebastian on 31.03.20.
//
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
/**
* allows time/velocity-constraint planning for cartesian paths
*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
ros::Duration(5.0).sleep();
std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
node_handle.setParam("tud_planner_ready", true);
// Visualization Setup
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
moveit::planning_interface::MoveGroupInterface::Plan plan;
group.setStartStateToCurrentState();
std::vector<geometry_msgs::Pose> waypoints;
// initial pose of the robot
geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
// waypoints for a circle
geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose;
geometry_msgs::Pose target_pose_4 = group.getCurrentPose().pose;
geometry_msgs::Pose target_pose_5 = group.getCurrentPose().pose;
// specification of the circles waypoints
target_pose_1.position.z = start_pose.position.z - 0.4;
target_pose_1.position.y = start_pose.position.y;
target_pose_1.position.x = start_pose.position.x + 0.5;
waypoints.push_back(target_pose_1);
target_pose_2.position.z = 0.6;//0.583;
target_pose_2.position.y = -0.6;//63;
target_pose_2.position.x = 0;//-0.007;
waypoints.push_back(target_pose_2);
target_pose_3.position.z = 0.6;//0.691;
target_pose_3.position.y = -0.032;
target_pose_3.position.x = -0.607;
waypoints.push_back(target_pose_3);
target_pose_4.position.z = 0.6;
target_pose_4.position.y = 0.6;//0.509;
target_pose_4.position.x = 0;//0.039;
waypoints.push_back(target_pose_4);
target_pose_5.position.z = target_pose_1.position.z;
target_pose_5.position.y = target_pose_1.position.y;
target_pose_5.position.x = target_pose_1.position.x;
waypoints.push_back(target_pose_5);
moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
// The trajectory needs to be modified so it will include velocities as well.
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// Third create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg);
//std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
// adjust velocities, accelerations and time_from_start
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
{
if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
//trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
}
}
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
{
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
//std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
group.execute(plan);
ros::shutdown();
return 0;
}
\ No newline at end of file
...@@ -4,33 +4,86 @@ ...@@ -4,33 +4,86 @@
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h> #include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h> #include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h> #include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h> #include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h> #include <trajectory_msgs/JointTrajectoryPoint.h>
/** std::vector<geometry_msgs::Pose> raw_trajectory;
* allows time/velocity-constraint planning for cartesian paths
*/ const double default_velocity = 1.0;
bool isInitialized = false;
void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface* group)
{
geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_4 = group->getCurrentPose().pose;
target_pose_1.position.z = 0.6;//0.583;
target_pose_1.position.y = -0.6;//63;
target_pose_1.position.x = 0;//-0.007;
raw_trajectory.push_back(target_pose_1);
target_pose_2.position.z = 0.6;//0.691;
target_pose_2.position.y = -0.032;
target_pose_2.position.x = -0.607;
raw_trajectory.push_back(target_pose_2);
target_pose_3.position.z = 0.6;
target_pose_3.position.y = 0.6;//0.509;
target_pose_3.position.x = 0;//0.039;
raw_trajectory.push_back(target_pose_3);
target_pose_4.position.z = group->getCurrentPose().pose.position.z;
target_pose_4.position.y = group->getCurrentPose().pose.position.y;
target_pose_4.position.x = group->getCurrentPose().pose.position.x;
raw_trajectory.push_back(target_pose_4);
}
void moveRobotToInitialState(ros::NodeHandle node_handle)
{
ROS_INFO("Moving to initial pose of trajectory.");
moveit::planning_interface::MoveGroupInterface group("panda_arm");
ros::Publisher display_publisher = node_handle.
advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
group.setStartStateToCurrentState();
std::vector<geometry_msgs::Pose> trajectory_to_init_pose;
geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
target_pose_1.position.z = group.getCurrentPose().pose.position.z - 0.4;
target_pose_1.position.y = group.getCurrentPose().pose.position.y;
target_pose_1.position.x = group.getCurrentPose().pose.position.x + 0.5;
trajectory_to_init_pose.push_back(target_pose_1);
group.setPlanningTime(10.0);
moveit_msgs::RobotTrajectory trajectory;
double fraction = group.computeCartesianPath(trajectory_to_init_pose, 0.01, 0.0, trajectory);
moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
cartesian_plan.trajectory_ = trajectory;
group.execute(cartesian_plan);
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
ros::init(argc, argv, "move_group_interface_tutorial"); ros::init(argc, argv, "timed_cartesian_planner");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
// wait for robot init of robot_state_initializer // wait for robot init of robot_state_initializer
std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
ros::Duration(5.0).sleep(); ros::Duration(5.0).sleep();
std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl; ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
node_handle.setParam("tud_planner_ready", true); node_handle.setParam("tud_planner_ready", true);
// Visualization Setup // Visualization Setup.
namespace rvt = rviz_visual_tools; namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers(); visual_tools.deleteAllMarkers();
...@@ -39,46 +92,40 @@ int main(int argc, char **argv) ...@@ -39,46 +92,40 @@ int main(int argc, char **argv)
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75; text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE); visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger(); visual_tools.trigger();
// Initialize start state of robot and target trajectory.
moveit::planning_interface::MoveGroupInterface group("panda_arm"); moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveRobotToInitialState(node_handle);
initRawTrajectory(node_handle, &group);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; for(int i = 0; i < raw_trajectory.size(); i++)
{
double velocity = 0.0;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); if(!node_handle.getParam("robot_speed_factor", velocity))
moveit_msgs::DisplayTrajectory display_trajectory; {
velocity = default_velocity;
}
moveit::planning_interface::MoveGroupInterface::Plan plan; std::cout << ">>>>>>>>>>>>>>>>> starting planing step " << i << " (speed: " << velocity << ")" << std::endl;
group.setStartStateToCurrentState();
std::vector<geometry_msgs::Pose> waypoints; std::vector<geometry_msgs::Pose> single_traj;
single_traj.push_back(raw_trajectory.at(i));
geometry_msgs::Pose start_pose = group.getCurrentPose().pose; moveit::planning_interface::MoveGroupInterface group("panda_arm");
geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose; moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
target_pose_1.position.z = start_pose.position.z - 0.2; ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
target_pose_1.position.y = start_pose.position.y + 0.2;
target_pose_1.position.x = start_pose.position.x + 0.2;
waypoints.push_back(target_pose_1);
geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose; moveit::planning_interface::MoveGroupInterface::Plan plan;
target_pose_2.position.z = start_pose.position.z + 0.2; group.setStartStateToCurrentState();
target_pose_1.position.y = start_pose.position.y - 0.2;
target_pose_2.position.x = start_pose.position.x - 0.2;
waypoints.push_back(target_pose_2);
moveit_msgs::RobotTrajectory trajectory_msg; moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0); group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false); double fraction = group.computeCartesianPath(single_traj,0.01,0.0,trajectory_msg, false);
// The trajectory needs to be modified so it will include velocities as well.
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm"); robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// Third create a IterativeParabolicTimeParameterization object // Third create a IterativeParabolicTimeParameterization object
...@@ -87,18 +134,19 @@ int main(int argc, char **argv) ...@@ -87,18 +134,19 @@ int main(int argc, char **argv)
bool success = iptp.computeTimeStamps(rt); bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success ? "SUCCEDED":"FAILED"); ROS_INFO("Computed time stamp %s",success ? "SUCCEDED":"FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg); rt.getRobotTrajectoryMsg(trajectory_msg);
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)",fraction * 100.0);
//std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl; //std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
// adjust velocities, accelerations and time_from_start // adjust velocities, accelerations and time_from_start
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{ {
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2); double new_tfs = 1 / velocity;
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
{ {
if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){ if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2; trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
//trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
} }
} }
...@@ -108,13 +156,16 @@ int main(int argc, char **argv) ...@@ -108,13 +156,16 @@ int main(int argc, char **argv)
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j))); sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
} }
} }
//std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl; //std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
group.execute(plan); group.execute(plan);
if(i == (raw_trajectory.size() -1))
{
i = -1;
}
}
ros::shutdown(); ros::shutdown();
return 0; return 0;
} }
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment