Moveit Velocity control using cartesianPath at real robot.
I had control a real robot(fanuc m710ic_50) with Moveit. And i created a plan path using computeCartesianPath , its worked in my robot. But the robot moves too slowly ,so i tried to speed up my robot. I changed some parameters usingarm.setMaxVelocityScalingFactor(0.6)
and arm.setMaxAccelerationScalingFactor(0.4)
,but it didnt work.
Then I created a function to try to redefine the parameters in my plan path, like
void scale_trajectory_speed(moveit::planning_interface::MoveGroupInterface::Plan &plan, double scale)
{
int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();
for(int i=0; i<plan.trajectory_.joint_trajectory.points.size(); i++)
{
plan.trajectory_.joint_trajectory.points[i].time_from_start *= 1/scale;
for(int j=0; j<n_joints; j++)
{
plan.trajectory_.joint_trajectory.points[i].velocities[j] *= scale;
plan.trajectory_.joint_trajectory.points[i].accelerations[j] *= scale*scale;
}
}
}
but it didnt work at all. Then i tried to use trajectory_processing::IterativeParabolicTimeParameterization::computeTimeStamps
to change time scale,like
moveit::planning_interface::MoveGroupInterface::Plan plan;
robot_trajectory::RobotTrajectory rt(arm.getCurrentState()->getRobotModel(),"manipulator");
rt.setRobotTrajectoryMsg(*arm.getCurrentState(),trajectory);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool ItSuccess = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s", ItSuccess ? "SUCCEDED" : "FAILED");
rt.getRobotTrajectoryMsg(trajectory);
plan.trajectory_=trajectory;
but i didnt work too. Am I using it the wrong way ?? Please help me ,I've been puzzling over this question for days. I am truly very grateful for your help. !!