MoveIT! acceleration scaling factor not working

asked 2018-05-22 07:52:01 -0600

Pulkit123 gravatar image

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 ??

edit retag flag offensive close merge delete

Comments

1

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-05-22 09:33:24 -0600 )edit