Moveit trajectory only providing translation, not velocity or acceleration

Question

I have just started using moveit and have implemented the multi-degree of freedom joint type described here https://www.wilselby.com/research/ros-integration/3d-mapping-navigation/. 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?

---

Originally posted by tonka46 on ROS Answers with karma: 46 on 2017-06-27

Post score: 1

---

Original comments

Comment by gvdhoorn on 2017-06-28:
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.

Comment by tonka46 on 2017-06-28:
Thanks! This will give me a start point I will have a look in the next few day.

Comment by gvdhoorn on 2017-06-29:
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.

Answer 1

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_;
        }

---

Originally posted by tonka46 with karma: 46 on 2017-07-07

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by 130s on 2017-12-09:
Update: Improvement for this is merged.