ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Moveit trajectory only providing translation, not velocity or acceleration

asked 2017-06-27 21:12:41 -0500

tonka46 gravatar image

I have just started using moveit and have implemented the multi-degree of freedom joint type described here https://www.wilselby.com/research/ros... . Though when I generate a trajectory it currently only publishes the transforms and not the velocities or accelerations. I assume this is something moveit can produce? Is this a flag in my launch file? or is it my lack of understanding of the custom join?

edit retag flag offensive close merge delete

Comments

I'm not sure, but it could be that the 'parameterisation step' (using the IPTP) is not run for multi-dof trajectories. Unfortunately I cannot offer you more assistance than this at the moment, but it should give you a starting point.

gvdhoorn gravatar image gvdhoorn  ( 2017-06-28 10:57:19 -0500 )edit

Thanks! This will give me a start point I will have a look in the next few day.

tonka46 gravatar image tonka46  ( 2017-06-28 17:39:00 -0500 )edit

I was about to suggest posting an issue over at ros-planning/moveit/issues, but I see you already posted one: ros-planning/moveit#39.

gvdhoorn gravatar image gvdhoorn  ( 2017-06-29 01:39:03 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2017-07-07 00:43:09 -0500

tonka46 gravatar image

I have resolved this for my needs by adding

in robot_trajectory.cpp

at line 284ish

        if (waypoints_[i]->hasVelocities()){
        const std::vector<std::string> names =  mdof[j]->getVariableNames();
        const double* velocities =  waypoints_[i]->getJointVelocities(mdof[j]);
         geometry_msgs::Twist point_velocity;
        for(std::size_t ii = 0; ii < names.size(); ++ii){
          if(names[ii].find("/x") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint x velocity: "<<velocities[ii]);
            point_velocity.linear.x = velocities[ii];
          }
          else if(names[ii].find("/y") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint y velocity: "<<velocities[ii]);
            point_velocity.linear.y = velocities[ii];
          }
          else if(names[ii].find("/z") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint z velocity: "<<velocities[ii]);
            point_velocity.linear.z = velocities[ii];
          }
          else if(names[ii].find("/theta") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint theta velocity: "<<velocities[ii]);
            point_velocity.angular.z = velocities[ii];
          }
          else
          {
            ROS_INFO_STREAM("velocity name: "<<names[ii]);
          }
        }
        trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
       }

in trajectory_execution_manager.cpp

at line 863

        if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
        {
          parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
        }
edit flag offensive delete link more

Comments

Update: Improvement for this is merged.

130s gravatar image 130s  ( 2017-12-09 09:22:58 -0500 )edit
0

answered 2017-06-28 12:21:51 -0500

Felix_N gravatar image

I am working with a 6DOF arm. For pretty much everything you need to know about moveit, see this tutorial: http://docs.ros.org/kinetic/api/movei...

After you plan a trajectory to a given goal, moveit will publish waypoints, accelerations and velocities on the topic /execute_trajectory/goal using a message of type moveit_msgs::ExecuteTrajectoryActionGoal

edit flag offensive delete link more

Comments

Thanks for the response I have the PR2 demo up and running and am currently trying to get a holonomic joint to work. I am getting waypoints just not the velocity or acceleration components.

tonka46 gravatar image tonka46  ( 2017-06-28 17:30:27 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2017-06-27 21:12:41 -0500

Seen: 750 times

Last updated: Jul 07 '17