Moveit time parameterization
Hello,
I used Moveit to compute a path with my own robot, and now I am trying to get timestamps in order to create velocity curves.
I use the max_velocity_scaling_factor from the MotionPlanRequest msg to set the velocity, and I use the MotionPlanResponse msg to read the path. I tried several "getDuration" functions from the RobotTrajectory ()class to read the path, but I always get a correct position vector, and a time vector filled with 0.
When I run
const vector<string> &adapter = planning_pipeline.getAdapterPluginNames();
I get an empty vector (I use this after generatePlan(), it is supposed to return the names of the planning adapters used).
I followed the Moveit tutorials about time parameterization and the planning request adapters (I run OMPL as a preprocessor or CHOMP), but it didn't make a change :
http://docs.ros.org/melodic/api/movei... http://docs.ros.org/melodic/api/movei...
I didn't install Moveit from source as specified in the tuto because CHOMP is now part of the official release and I use it, plus another person had the same problem with a source build (but didn't get an answer) : https://github.com/ros-planning/movei...
Thanks
Edit :
I tried to run OMPL as a preprocessor for STOMP according to the tutorial about planning adapters, but It seems that it doesn't take these settings, I still get CHOMP messages while running the code.
This is the code :
robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model
= robot_model_loader.getModel();
static const string group = "Leg"; moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::MoveGroupInterface move_group(group); const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(group); moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); map<string, double> joint_goal;
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); planning_pipeline::PlanningPipelinePtr planning_pipeline(
new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));
//------------------------FK--------------------------//
moveit::core::RobotState target_state = *current_state;
map<string, double> joint_values = move_group.getNamedTargetValues("Plie_Rent");
vector<double> j1 = {joint_values["J1"]};
target_state.setJointPositions("J1",j1);
vector<double> j2 = {joint_values["J2"]};
target_state.setJointPositions("J2",j2);
vector<double> j3 = {joint_values["J3"]};
target_state.setJointPositions("J3",j3);
const Eigen::Isometry3d eef_state = target_state.getGlobalLinkTransform("Bout_pied");
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
double deg_goal_speed = 10*360.0;
double rad_goal_speed = deg_to_rad(deg_goal_speed);
double speed_scale = rad_goal_speed/20;
req.max_velocity_scaling_factor = speed_scale;
req.group_name = "Leg";
moveit_msgs::Constraints joint_goal1 = kinematic_constraints::constructGoalConstraints(target_state, > joint_model_group, 0.1, 0.1);
req.goal_constraints.push_back(joint_goal1);
//--------------------Planning--------------------//
planning_pipeline->generatePlan(planning_scene, req, res);
const vector<string> &adapter = planning_pipeline->getAdapterPluginNames();
cout<<"ADAPTER_VECTOR_SIZE : "<<adapter.size()<<endl;
/* Check that the planning was successful */
if (res.error_code_.val != res.error_code_.SUCCESS) {
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
size_t nb_points = res.trajectory_->getWayPointCount();
const uint size = nb_points;
double duree = res.trajectory_->getWayPointDurationFromStart(size);
cout<<"TOTAL DURATION = "<<duree<<endl;
double velocities_j1[size];
double time_j1[size];
double positions_j1[size];
// Method with MotionPlanRequest :
velocities_j1[0] = 0.0;
time_j1[0] = 0.0;
for (int j = 1; j<=size-1; ++j){
const robot_state::RobotState& points_ptr = res.trajectory_->getWayPoint(j);
time_j1[j] = time_j1[j-1] + res.trajectory_->getWayPointDurationFromPrevious(j);
const string joint = "J1";
const string& joint_ptr = joint;
const double position = *points_ptr.getJointPositions(joint);
const double vel1 = *points_ptr.getJointVelocities(joint_ptr);
velocities_j1[j ...
Do you have issues if you don't use CHOMP? Are there large jumps in your trajectory? How are you calling the retiming functions and what is returned?
Thank you for your answer fvd, I tried to run OMPL as a preprocessor for STOMP according to the tutorial about planning adapters, but It seems that it doesn't take these settings, I still get CHOMP messages while running the code, this is the console output :
<[ INFO] [1585590007.623463814]: Ready to take commands for planning group Leg.
What do you mean by "large jumps" ? I set a joint goal and use the function generatePlan(), it generates a path with 100 points for each joint. This is the code that calls the retiming functions :
planning_pipeline->generatePlan(planning_scene, req, res); size_t nb_points = res.trajectory_->getWayPointCount(); const uint size = nb_points; double velocities_j1[size]; double time_j1[size]; for (int j = 0; j<=size-1; ++j){
const robot_state::RobotState& points_ptr = res.trajectory_->getWayPoint(j); time_j1[j] = res.trajectory_->getWayPointDurationFromPrevious(j); const string joint = "J1"; const string& joint_ptr = joint; const double position = *points_ptr.getJointPositions(joint); const double vel1 = *points_ptr.getJointVelocities(joint_ptr); velocities_j1[j] = vel1; } for (int l = 0; l<=size-1; ++l){ cout<<time_j1[l]<<endl; };<="" p="">
I also tried with getWayPointDurationFromStart() and getDuration(), but it also gave vectors filled with zeros.
Please edit your question with the extra information. In your code, I don't actually see where you are using the time parametrization. It looks like you are just reading the trajectory, but it's hard to read as is.
By "large jumps" I mean large differences between the waypoints of the trajectory. If you graph it or print it out, does it look continuous?
There is no large jump, it is continuous. I set the velocity with this method (http://docs.ros.org/melodic/api/movei...) :
planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; double deg_goal_speed = 10*360.0; // °/s double rad_goal_speed = deg_to_rad(deg_goal_speed);// rad/s double speed_scale = rad_goal_speed/20; req.max_velocity_scaling_factor = speed_scale; moveit_msgs::Constraints joint_goal1 = kinematic_constraints::constructGoalConstraints(target_state, joint_model_group, 0.1, 0.1); req.goal_constraints.push_back(joint_goal1); planning_pipeline->generatePlan(planning_scene, req, res);
I edited the ompl_planning_pipeline.launch file and some other files as specified there to use AddTimeParameterization : http://docs.ros.org/melodic/api/movei...
Do you mean that there is a way to call the adapter programmatically ? Isn't it called automatically with the planner, if the files are configured ?
I edited your question. Please add new information to it yourself. Yes, you can call the time parametrization directly with a
RobotTrajectory
of your own. Look here for function of the IPTP algorithm. The others are analogous.I'm confused I edited it, what did you edit ?
I feel stupid, I didn't use a IterativeParabolicTimeParameterization to compute the stamps before using getWayPointDuration(), it works now ! Thank you a lot !