Hi
I am using ROS Indigo on Ubuntu 14.04. I am trying to move a 7 axis manipulator arm in Cartesian path in only "-X" direction, in small steps keeping the step size constant (0.0001 m). However I am seeing a jerky motion throughout the execution of whole event. I have tried using **IterativeParabolicTimeParameterization** and **spline trajectory** but the jerky motion still exists. I want to get a smooth trajectory execution. Can anyone tell me what I am doing wrong here ??
Here's my code :
int main(int argc, char **argv)
{
ros::init(argc, argv, "circle");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// sleep(5.0);
moveit::planning_interface::MoveGroup group("arm");
moveit::planning_interface::PlanningSceneInterface plannning_scene_interface;
tf::TransformListener listener;
tf::StampedTransform transform;
group.setGoalPositionTolerance(0.0001);
group.setGoalOrientationTolerance(0.001);
group.setPlannerId("RRTConnectkConfigDefault");
robot_model_loader::RobotModelLoader model_loader("robot_description");
std::vector joint_positions;
joint_positions.resize(7);
geometry_msgs::PoseStamped pose;
joint_positions[0] = -2.277;
joint_positions[1] = -1.0;
joint_positions[2] = 0.0;
joint_positions[3] = -0.80;
joint_positions[4] = 0.0;
joint_positions[5] = -0.8;
joint_positions[6] = 0;
group.setJointValueTarget(joint_positions);
std::vector waypoints;
group.move();
sleep(2.0);
listener.lookupTransform("world", group.getEndEffectorLink().c_str(),
ros::Time(0), transform);
moveit::planning_interface::MoveGroup::Plan my_plan;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = "/world";
pose.pose.position.x = transform.getOrigin().getX();
pose.pose.position.y = transform.getOrigin().getY();
pose.pose.position.z = transform.getOrigin().getZ();
pose.pose.orientation.x = transform.getRotation().getX();
pose.pose.orientation.y = transform.getRotation().getY();
pose.pose.orientation.z = transform.getRotation().getZ();
pose.pose.orientation.w = transform.getRotation().getW();
geometry_msgs::Pose pose2 = pose.pose;
group.setMaxVelocityScalingFactor(0.50);
group.setMaxAccelerationScalingFactor(0.50);
robot_trajectory::RobotTrajectory rt(model_loader.getModel(), "arm");
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.0001;
for (int i = 0; i <= 900; i++) {
pose2.position.x -= 0.001;
waypoints.clear();
waypoints.push_back(pose2);
double fraction = group.computeCartesianPath(waypoints, eef_step,
jump_threshold, trajectory);
// ROS_INFO("Visualize the plan (%.2f%% achieved)", fraction*100.0);
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(robot_state::RobotState(model_loader.getModel()),
trajectory);
// Thrid create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
trajectory_processing::SplineParameterization sp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt, 0.5, 0.5);
// bool success = sp.computeTimeStamps(rt, 0.5, 0.5);
ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
// Get RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory);
// Finally plan and execute the trajectory
my_plan.trajectory_ = trajectory;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",
fraction * 100.0);
sleep(5.0);
group.execute(my_plan);
// sleep(2.0); }
ros::shutdown();
return 0;
}
}
↧