MoveIT! acceleration scaling factor not working
I am using ROS Indigo with Ubuntu 14.04. During one of my codes I set my acceleration scaling factor to be 0.001 and velocity scaling factor to be 0.005 for testing purposes. But when I recorded the data with follow_joint_trajectory/goal topic, the scaled acceleration limits were violated. Here's my code :
#include <moveit/move_group_interface/move_group.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 <tf/transform_listener.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_cartesian_demo");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
sleep(5.0);
moveit::planning_interface::MoveGroup group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Setting the pose tolerance
group.setGoalPositionTolerance(0.00001);
// Setting the orientation tolerance
group.setGoalOrientationTolerance(0.001);
tf::TransformListener listener;
tf::StampedTransform transform;
std::vector<double> joint_positions;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), joint_positions);
joint_positions[0] = 0.0;
joint_positions[1] = -0.54168;
joint_positions[2] = 0.0;
joint_positions[3] = -0.66667;
joint_positions[4] = 0.000139;
joint_positions[5] = -0.272563;
joint_positions[6] = 0;
group.setJointValueTarget(joint_positions);
group.setJointValueTarget(joint_positions);
moveit::planning_interface::MoveGroup::Plan my_plan;
group.move();
sleep(5.0);
// ......................... Cartesian Paths ..........................
std::vector<geometry_msgs::Pose> waypoints;
listener.lookupTransform ("base_link", group.getEndEffectorLink().c_str(), ros::Time(0), transform );
geometry_msgs::PoseStamped pose;
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 target_pose2 = pose.pose;
target_pose2.position.x += 0.001;
// Note this is not the speed of the end-effector point.
group.setMaxVelocityScalingFactor(0.005);
group.setMaxAccelerationScalingFactor(0.001);
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.0001;
double fraction = group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO("Visualizing plan 2 (cartesian path) (%.2f%% achieved)", fraction*100.0);
my_plan.trajectory_ = trajectory;
group.execute(my_plan);
ros::shutdown();
return 0;
}
The maximum acceleration for each joint is set to be 10 rad/s^2 and maximum velocity is set to be pi radians/s. can anyone help me out with this ??
I'm not sure about the rest of your question, but at least for
computeCartesianPath(..)
there is no support for scaling velocities/accelerations. See #q288989.