Moveit: Moveit! trajectory velocity has weird behavior on kinetic

[renarded]

Description

Hi everyone,

I encountered a problem using Moveit! with ros kinetic. I first developped under indigo and switched to kinetic after first kinetic Moveit! release in december.

After testing some movements with my custom robot (6 axis arm), I noticed that sometimes (like 30-50%), the trajectory calculated by Moveit! does not have a correct velocity. I’m not talking about how smooth the velocity is, it’s not the problem here. Sometimes the velocity just goes up or down.

Your environment

• ROS Distro: Kinetic
• OS Version: Ubuntu 16.04
• Source or Binary build : binary
• If binary, which release version? last release version on January, 16th 2017

Steps to reproduce

For the test I made I asked the robot to do a very basic movement. On indigo, every time, the velocity (of each joint) increases, then reaches the max velocity (or not, depending on the planning time and max acceleration put in the config file), and finally decreases. That is, I think, the correct behavior for the robot.

However, on kinetic, with the exact same URDF and SRDF, the trajectory sometimes has a “weird” velocity : the velocity increases, and sometimes just decreases, then increases, 2 or 3 times in one trajectory.

Here’s my configuration for the test I made :

URDF (I did not put any visual or collision for simplicity, the problem I have is not related to that)

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from test.urdf.xacro                | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="test_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <link name="ground_link"/>
  <link name="base_link"/>
  <link name="shoulder_link"/>
  <link name="arm_link"/>
  <link name="elbow_link"/>
  <link name="forearm_link"/>
  <link name="wrist_link"/>
  <link name="hand_link"/>
  <link name="gripper_link"/>
  <joint name="ground_joint" type="fixed">
    <parent link="ground_link"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="shoulder_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.085"/>
    <axis xyz="0 0 1"/>
    <limit effort="1" lower="-3.14159" upper="3.14159" velocity="1.0"/>
  </joint>
  <joint name="joint_2" type="revolute">
    <parent link="shoulder_link"/>
    <child link="arm_link"/>
    <origin rpy="1.570795 -1.570795 0" xyz="0 0 0.077"/>
    <limit effort="1" lower="-2.07" upper="1.35" velocity="1.0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="joint_3" type="revolute">
    <parent link="arm_link"/>
    <child link="elbow_link"/>
    <origin rpy="0 0 0" xyz="0.18475 0.0045 0"/>
    <limit effort="1" lower="-2.9" upper="-0.65" velocity="1.0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="joint_4" type="revolute">
    <parent link="elbow_link"/>
    <child link="forearm_link"/>
    <origin rpy="0 1.570795 0" xyz="0.045 0.02901 0"/>
    <limit effort="1" lower="-2.270795" upper="2.270795" velocity="1.0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="joint_5" type="revolute">
    <parent link="forearm_link"/>
    <child link="wrist_link"/>
    <origin rpy="0 -1.570795 0" xyz="0 0 0.151"/>
    <limit effort="1" lower="-1.570795" upper="1.570795" velocity="1.0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="joint_6" type="revolute">
    <parent link="wrist_link"/>
    <child link="hand_link"/>
    <origin rpy="0 1.570795 0" xyz="0.02518 -0.00236 0"/>
    <limit effort="1" lower="-1.570795" upper="1.570795" velocity="1.0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="hand_gripper_joint" type="fixed">
    <parent link="hand_link"/>
    <child link="gripper_link"/>
    <origin rpy="-1.570795 -1.570795 0" xyz="0 0 0.045"/>
  </joint>
</robot>

SRDF

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="test_robot">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="arm">
        <chain base_link="base_link" tip_link="gripper_link" />
    </group>
    <group name="gripper">
        <link name="gripper_link" />
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="standard_pose" group="arm">
        <joint name="joint_1" value="0" />
        <joint name="joint_2" value="0" />
        <joint name="joint_3" value="-1.76" />
        <joint name="joint_4" value="0" />
        <joint name="joint_5" value="0" />
        <joint name="joint_6" value="0" />
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="gripper_eef" parent_link="gripper_link" group="gripper" />
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="ground_link" />
</robot>

My robot is a 6 robotic arm, so I have 6 joints (joint_1, …, joint_6). I made a simple test to move the robot with the set_pose_target command (moveit_commander). I ask the robot to go to pose 1 (0.3, 0.0, 0.3) then to pose 2 (0.3, 0.0, 0.0) with the same orientation (0,0,0,1).
Script to run the demo :

#!/usr/bin/env python

import rospy, sys
import moveit_commander

from geometry_msgs.msg import PoseStamped, Pose


    class TestWeirdVelocity():

    def test_target_pose_1(self, reference_frame):
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = 0.30 
        target_pose.pose.position.y = 0.0 
        target_pose.pose.position.z = 0.3
        target_pose.pose.orientation.x = 0.0
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.z = 0.0
        target_pose.pose.orientation.w = 1.0
        return target_pose

    def test_target_pose_2(self, reference_frame):
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = 0.30 
        target_pose.pose.position.y = 0.0 
        target_pose.pose.position.z = 0.0
        target_pose.pose.orientation.x = 0.0
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.z = 0.0
        target_pose.pose.orientation.w = 1.0
        return target_pose

    def log_traj(self, traj):
        rospy.loginfo("TRAJECTORY : ")
        rospy.loginfo(traj)

    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        arm = moveit_commander.MoveGroupCommander('arm')
        end_effector_link = arm.get_end_effector_link()

        reference_frame = 'ground_link'
        arm.set_pose_reference_frame(reference_frame)

        arm.allow_replanning(True)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)

        # test multiple times
        for i in range(0,6):
            # go bottom
            target_pose_1 = self.test_target_pose_1(reference_frame)
            arm.set_start_state_to_current_state()
            arm.set_pose_target(target_pose_1, end_effector_link)
            traj = arm.plan()
            self.log_traj(traj)
            arm.execute(traj)
            rospy.sleep(2)

            # go top
            target_pose_2 = self.test_target_pose_2(reference_frame)
            arm.set_start_state_to_current_state()
            arm.set_pose_target(target_pose_2, end_effector_link)
            traj = arm.plan()
            self.log_traj(traj)
            arm.execute(traj)
            rospy.sleep(2)


        rospy.sleep(1)

        # shut down moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)


if __name__ ==  "__main__":
    rospy.init_node('test_weird_velocity')
    TestWeirdVelocity()

Expected behaviour

Every time the velocity (for each joint) should increase (and only increase), then (optional) stay constant, then decrease (and only decrease). That's the result I get on indigo.

Actual behaviour

The demo mainly involves joint_2, so I have plotted joint_2 velocity here :

This is the velocity of joint_2 directly taken from Moveit! trajectory (I’ve just used a cubic interpolator on the trajectory, which doesn't modify its main behavior). If you test and look at the logged trajectory, you can see that sometimes the velocity just decreases in the middle.

Here on the plot, for trajectories 3,4,7,8 and 10, you can see that the velocity is not ok. That is the behavior that I have on kinetic, and not indigo.

Here is the log of one trajectory with a "not normal" velocity behavior :

[INFO] [1484570660.822931]: joint_trajectory: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: /ground_link
  joint_names: ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
  points: 
    - 
      positions: [0.0009942702889553223, -0.24519055847849927, -1.7050523598659837, 0.1425924785274416, 0.356846956328268, -0.10039826258595985]
      velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
      accelerations: [-0.02004846917210448, 0.0, 0.0, 0.0, 0.0, 0.0]
      effort: []
      time_from_start: 
        secs: 0
        nsecs:         0
    - 
      positions: [-0.0007167963266594883, -0.32834064519752953, -1.6958775312815293, 0.13204366013985191, 0.43503627659424077, -0.0907050003653197]
      velocities: [-0.007129554664912496, -0.34646406121513346, 0.038229044583729936, -0.04395409077485364, 0.3257938808234545, 0.04038921819449639]
      accelerations: [-0.020526914244262697, -0.99751504933153, 0.11006638656861602, -0.12654953842492697, 0.9380029142466321, 0.11628580707171245]
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 413150086
    - 
      positions: [-0.0024278629422742988, -0.4114907319165598, -1.6867027026970747, 0.12149484175226219, 0.5132255968602135, -0.08101173814467955]
      velocities: [-0.011634049872199315, -0.565362123799783, 0.06238238321426783, -0.07172454777273235, 0.5316324000166632, 0.06590736741053911]
      accelerations: [-0.02027127663315851, -0.9850922194206546, 0.1086956443425453, -0.12497351870251065, 0.9263212351908593, 0.11483760957006194]
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 582267990
    - 
      positions: [-0.0041389295578891094, -0.4946408186355901, -1.6775278741126203, 0.11094602336467248, 0.5914149171261862, -0.07131847592403939]
      velocities: [-0.014362351865941364, -0.6979452420168616, 0.07701168103983094, -0.0885446773783483, 0.6563055579312886, 0.08136330957029808]
      accelerations: [-0.020199183987483143, -0.9815888434065355, 0.10830907981009542, -0.12452906363608097, 0.9230268719497777, 0.11442920178959723]
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 712382139
    - 
      positions: [-0.005849996173503919, -0.5777909053546204, -1.6683530455281659, 0.10039720497708277, 0.6696042373921589, -0.06162521370339924]
      velocities: [-0.01522699432188556, -0.7493792431108848, 0.08206098197844001, -0.09497793083997069, 0.7050895969342986, 0.08721253862454442]
      accelerations: [0.007084754428651291, 0.1521468340618152, -0.029560654056478987, 0.021177513306264106, -0.13452792108797182, -0.020730319608780197]
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 822247598
    - 
      positions: [-0.007132102181192561, -0.6417181457447368, -1.6614071470575444, 0.09230292774900808, 0.7297896923124505, -0.054198147401839744]
      velocities: [-0.014108448125936524, -0.7034630127848684, 0.07643349900329188, -0.08907039644492, 0.6622879571476377, 0.08172832746673325]
      accelerations: [0.016925106780505943, 0.8439047655165108, -0.0916929431707102, 0.10685271387438115, -0.7945093814790519, -0.0980448492292838]
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 908411859
    - 
      positions: [-0.008414208188881203, -0.7056453861348533, -1.654461248586923, 0.08420865052093339, 0.789975147232742, -0.04677108110028025]
      velocities: [-0.012411153191460002, -0.6246582334955628, 0.06749376442547686, -0.0790369633167953, 0.5883481246210913, 0.07248434495900058]
      accelerations: [0.019587859530720255, 0.8534651132836523, -0.10071413219853784, 0.10923529123883018, -0.798171870004432, -0.1010265327055608]
      effort: []
      time_from_start: 
        secs: 1
        nsecs:   4542599
    - 
      positions: [-0.009481833892606761, -0.7599612033605129, -1.648629815173218, 0.07734164387261622, 0.8411586694801929, -0.04047711275826108]
      velocities: [-0.01057234154672009, -0.5378714366795637, 0.057746741710608, -0.06800167833751347, 0.5068533633799621, 0.062327070961168456]
      accelerations: [0.017944656089056033, 0.9129404218250518, -0.09801475062843369, 0.11542066871873188, -0.8602927982643886, -0.10578903911625208]
      effort: []
      time_from_start: 
        secs: 1
        nsecs:  97499257
    - 
      positions: [-0.010549459596332317, -0.8142770205861725, -1.6427983817595129, 0.07047463722429903, 0.8923421917276438, -0.034183144416241915]
      velocities: [-0.008696837688636787, -0.45820150953387795, 0.048193366839539606, -0.05778240316726019, 0.4324465321776537, 0.052860779862099305]
      accelerations: [0.0199405816948342, 0.6882959634317997, -0.09460848023170482, 0.09006062725164868, -0.6347520168859976, -0.08461242021794603]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 208025417
    - 
      positions: [-0.011188124692131707, -0.8493699914829183, -1.6391958784596408, 0.06606217173549689, 0.9255218486294136, -0.030155371993303405]
      velocities: [-0.006968252754541723, -0.38288720132767773, 0.039305660678323, -0.04814287644478356, 0.36201169771320885, 0.04394562418614534]
      accelerations: [0.016511838186748826, 0.9072821745708222, -0.09313794028506196, 0.11407843740782564, -0.8578159812665762, -0.10413270889239232]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 290602261
    - 
      positions: [-0.011826789787931096, -0.884462962379664, -1.6355933751597687, 0.06164970624669473, 0.9587015055311833, -0.026127599570364892]
      velocities: [-0.005133758981366383, -0.29250251517171566, 0.029414766198064442, -0.03668831227391793, 0.27696442284314216, 0.03342825237452923]
      accelerations: [0.020616807255721705, 0.9318734865175886, -0.10747743195602726, 0.11890528680873032, -0.8731651929423416, -0.10972443189695891]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 393574322
    - 
      positions: [-0.0122509745797674, -0.9099445101119529, -1.633105336916813, 0.05846451133765009, 0.9828792297601123, -0.023232925106966708]
      velocities: [-0.0035515093661784352, -0.21334559177423418, 0.020831230381963863, -0.026668211049257474, 0.20242926126653263, 0.02423581341587438]
      accelerations: [0.008421903436675409, 0.5059189734015467, -0.04939832410826992, 0.06323990031522907, -0.48003243561165776, -0.057471812475440406]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 497919599
    - 
      positions: [-0.012675159371603704, -0.9354260578442417, -1.6306172986738572, 0.05527931642860545, 1.0070569539890415, -0.020338250643568527]
      velocities: [-0.001502886109592294, -0.11223963640483106, 0.00977832108209859, -0.013856575275326532, 0.10728632435421948, 0.01247352089087482]
      accelerations: [0.021268306378182965, 0.9733632607569688, -0.1114019884890515, 0.12407293120700111, -0.9126166673379881, -0.11440796394462326]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 637554426
    - 
      positions: [-0.01267038355551384, -0.9416847592476167, -1.6303581905447344, 0.054548662679075835, 1.0132308128722893, -0.019709772099251004]
      velocities: [6.733192119645833e-05, -0.08823840402451213, 0.0036530401931687486, -0.010301133829176043, 0.08704225036787477, 0.008860615028593831]
      accelerations: [0.0007217199857884965, -0.9458131977663078, 0.03915634767951988, -0.11041618935996426, 0.9329918199602136, 0.09497550105328721]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 786598728
    - 
      positions: [-0.012665607739423976, -0.9479434606509919, -1.6300990824156116, 0.05381800892954622, 1.0194046717555372, -0.01908129355493348]
      velocities: [0.00011064620042628391, -0.1450017163256761, 0.006003022195063714, -0.016927800336416637, 0.14303608316266553, 0.014560603186910184]
      accelerations: [0.0003699037788422443, -0.48475846979682985, 0.02006883730189476, -0.05659170660902409, 0.4781871177575007, 0.04867787705596675]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 833137157
    - 
      positions: [-0.012660831923334111, -0.954202162054367, -1.6298399742864889, 0.0530873551800166, 1.025578530638785, -0.018452815010615957]
      velocities: [0.00010128858061641875, -0.1327385665028324, 0.0054953319246750735, -0.015496174856685025, 0.13093917174434352, 0.013329177360260031]
      accelerations: [-0.0007156193075788785, 0.9378182661044653, -0.03882536020282809, 0.10948284449823044, -0.9251052670348716, -0.09417267588414403]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 873381163
    - 
      positions: [-0.012656056107244245, -0.9604608634577421, -1.6295808661573663, 0.05235670143048699, 1.0317523895220329, -0.01782433646629843]
      velocities: [0.004645271805080455, -0.12531903838332348, -0.005789513601787032, -0.009279596186118413, 0.1318218580609439, 0.01015982614657886]
      accelerations: [0.10408925652355604, -0.35052859013630516, -0.2359961899856211, 0.08117355675335707, 0.532936167300504, -0.02012312625301389]
      effort: []
      time_from_start: 
        secs: 1
        nsecs: 930300033
    - 
      positions: [-0.011566286153221122, -0.9771128280053745, -1.63149028912926, 0.051679349149186024, 1.0501202412258261, -0.016726119900471625]
      velocities: [0.011963388385842709, -0.18280364450787745, -0.020961459362392777, -0.007435907353955235, 0.2016404865401336, 0.012056114467323319]
      accelerations: [0.05731276855998226, -0.87575381085173, -0.10041964954825593, -0.035623054561617525, 0.9659950981005292, 0.057756989567941765]
      effort: []
      time_from_start: 
        secs: 2
        nsecs:  48667885
    - 
      positions: [-0.010476516199197997, -0.9937647925530068, -1.6333997121011534, 0.05100199686788506, 1.0684880929296194, -0.01562790333464482]
      velocities: [0.016529119891016082, -0.25256919353726565, -0.028961232697389396, -0.010273761929978227, 0.27859496569122794, 0.016657234139955034]
      accelerations: [0.054218278025583676, -0.8284692013967369, -0.0949976875177816, -0.033699657595865164, 0.9138380873251993, 0.054638514191442236]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 122700471
    - 
      positions: [-0.009386746245174873, -1.010416757100639, -1.6353091350730469, 0.050324644586584096, 1.0868559446334127, -0.014529686768818016]
      velocities: [0.019814033423881466, -0.3027635152740462, -0.03471684133495673, -0.012315517318029266, 0.33396151751137226, 0.01996760844949016]
      accelerations: [0.05337256669588001, -0.8155465152575843, -0.0935158879558043, -0.033174001243913225, 0.899583794269322, 0.05378624790463592]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 182127015
    - 
      positions: [-0.00829697629115175, -1.0270687216482715, -1.6372185580449403, 0.04964729230528313, 1.1052237963372062, -0.013431470202991212]
      velocities: [0.023346248855841932, -0.3449573914736326, -0.04072932309850182, -0.012536703187770348, 0.3815928841678331, 0.022527726341271086]
      accelerations: [0.07702691112178654, -0.7357473607912697, -0.1283524152050099, 0.026078703515493438, 0.8523796755143851, 0.04018429931715138]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 233314037
    - 
      positions: [-0.006884708519150593, -1.0473387842460964, -1.639673422630851, 0.04898901231761965, 1.127703730267898, -0.012119388936703104]
      velocities: [0.02687166623918051, -0.38568490167004865, -0.04670948606403716, -0.012525301838021564, 0.42773282350585845, 0.024965385860512633]
      accelerations: [0.055740262026081226, -0.8000314266796041, -0.09689012096015699, -0.025981403616462295, 0.8872520016868654, 0.051786038761459194]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 288909589
    - 
      positions: [-0.005472440747149436, -1.067608846843921, -1.6421282872167617, 0.04833073232995618, 1.15018366419859, -0.010807307670414996]
      velocities: [0.029668358486337008, -0.42582539629904026, -0.05157081681968442, -0.013828883619363172, 0.4722494925023298, 0.027563680304255128]
      accelerations: [0.0556659300020873, -0.7989645505466615, -0.09676091383879777, -0.025946756303251235, 0.8860688131856806, 0.051716979863350526]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 338741200
    - 
      positions: [-0.00406017297514828, -1.0878789094417458, -1.6445831518026726, 0.04767245234229271, 1.1726635981292821, -0.00949522640412689]
      velocities: [0.0321138669739226, -0.460925404321337, -0.055821704859286024, -0.014968773184893492, 0.5111761538039414, 0.029835704021515877]
      accelerations: [0.050781036196067914, -0.72885241940921, -0.08826978131546072, -0.02366983127663102, 0.8083129568283818, 0.04717862121936581]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 384304227
    - 
      positions: [-0.002647905203147123, -1.1081489720395705, -1.6470380163885832, 0.047014172354629236, 1.1951435320599741, -0.008183145137838783]
      velocities: [0.03561554353016234, -0.4925166800962255, -0.06162885958114302, -0.013472127418478955, 0.548050025647296, 0.03150500764409844]
      accelerations: [0.09866720418325477, -0.6434595388850182, -0.1599339550455733, 0.08351831172003199, 0.7897083816695611, 0.02610430764579919]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 426801685
    - 
      positions: [-0.000590641795189902, -1.13565523073778, -1.6505837642025285, 0.046394036954240755, 1.2258476304444634, -0.006443334470628071]
      velocities: [0.03896844173649963, -0.5210203201614296, -0.06716313845160427, -0.011746531885664106, 0.5815934237394472, 0.0329552892232996]
      accelerations: [0.0366931856704974, -0.49059943107447046, -0.06324167453454367, -0.011060685422827062, 0.5476358440584822, 0.03103112396621323]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 480941237
    - 
      positions: [0.001466621612767319, -1.1631614894359896, -1.6541295120164738, 0.04577390155385227, 1.2565517288289527, -0.004703523803417358]
      velocities: [0.03840966529027755, -0.5135493033607623, -0.06620007248979656, -0.011578095965467803, 0.5732538368331822, 0.032482736599283114]
      accelerations: [-0.05696414778720296, 0.7616285690482062, 0.0981792130791327, 0.017171104321970512, -0.8501744556782765, -0.04817410915170677]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 532453163
    - 
      positions: [0.00352388502072454, -1.1906677481341994, -1.6576752598304192, 0.04515376615346378, 1.287255827213442, -0.0029637131362066456]
      velocities: [0.03530206936570382, -0.4719997686243702, -0.060844048845199714, -0.010641351436071422, 0.5268738105136095, 0.02985466840050905]
      accelerations: [-0.0541040665011741, 0.7233883828635694, 0.09324978745068925, 0.016308969873561577, -0.807488518207069, -0.045755362037809465]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 588233197
    - 
      positions: [0.005581148428681761, -1.218174006832409, -1.6612210076443645, 0.0445336307530753, 1.3179599255979313, -0.001223902468995933]
      velocities: [0.03217065497252404, -0.42551262133603784, -0.05537778666268859, -0.008923228356072167, 0.4754704726364977, 0.026814520872442328]
      accelerations: [-0.05059316843394621, 0.8270474717578461, 0.08945439863015107, 0.040492307708624366, -0.9072772039228633, -0.05556474966824834]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 649238999
    - 
      positions: [0.007469724749379863, -1.242855046517294, -1.6644674839125655, 0.04405984921949747, 1.3455705907584772, 0.00032490158162164565]
      velocities: [0.028598592718297226, -0.37374343524081255, -0.04916118641664277, -0.0071744440337122555, 0.4181065699880477, 0.023453442552794607]
      accelerations: [-0.06088105339804162, 0.7956298501186088, 0.10465496833443175, 0.015273049083923709, -0.89007066411448, -0.049927991299762324]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 710919107
    - 
      positions: [0.009358301070077965, -1.2675360862021794, -1.6677139601807665, 0.04358606768591963, 1.373181255919023, 0.0018737056322392243]
      velocities: [0.024112928595826803, -0.3151221060382777, -0.04145029755230621, -0.006049138795174979, 0.3525269221068207, 0.01977479070989526]
      accelerations: [-0.06229648567443476, 0.8141275617557607, 0.10708810659005545, 0.015628134376073983, -0.9107640436790782, -0.05108877427634967]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 781976191
    - 
      positions: [0.011246877390776067, -1.2922171258870647, -1.6709604364489674, 0.04311228615234179, 1.400791921079569, 0.003422509682856803]
      velocities: [0.019085683777722597, -0.24942307788600296, -0.03280842757991417, -0.004787968815710126, 0.2790294564065963, 0.01565199352537486]
      accelerations: [-0.05084694589397298, 0.6644981596510123, 0.08740626542127966, 0.012755822329961426, -0.743373716025379, -0.04169911212961107]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 869218188
    - 
      positions: [0.01313545371147417, -1.31689816557195, -1.6742069127171684, 0.04263850461876396, 1.4284025862401148, 0.004971313733474382]
      velocities: [0.013127760956658772, -0.1713012946892117, -0.022562819578458272, -0.0032497278817916987, 0.19166279461048424, 0.0107438862569911]
      accelerations: [-0.06549337876037208, 0.8609225552811888, 0.11265872101203155, 0.017270857748155455, -0.9625710273543459, -0.054136133511104315]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 983512442
    - 
      positions: [0.014041660221697968, -1.3286925711298019, -1.675763962665846, 0.04241928623153243, 1.4416022204989765, 0.005710375149983087]
      velocities: [0.012281412977163031, -0.15984432228405893, -0.021102004046581344, -0.0029709691062725302, 0.17888876062096892, 0.010016170011169865]
      accelerations: [0.06613120947829654, -0.8607070196678006, -0.1136270763479113, -0.015997652768926483, 0.9632548082159571, 0.05393365066467425]
      effort: []
      time_from_start: 
        secs: 3
        nsecs:  76631344
    - 
      positions: [0.014947866731921767, -1.3404869766876537, -1.6773210126145237, 0.042200067844300915, 1.454801854757838, 0.006449436566491793]
      velocities: [0.01605339947036192, -0.20893725856029494, -0.02758305589225586, -0.003883441910615934, 0.23383080923557836, 0.013092433146850527]
      accelerations: [0.0430544622306392, -0.5603598990896373, -0.07397645840130472, -0.010415208527902056, 0.6271232310128374, 0.03511328983427735]
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 137733068
    - 
      positions: [0.015854073242145564, -1.3522813822455055, -1.6788780625632014, 0.04198084945706939, 1.4680014890166997, 0.007188497983000499]
      velocities: [0.015319790167401506, -0.19938923000111874, -0.02632256359316238, -0.0037059761272226328, 0.2231451935632913, 0.012494134277340085]
      accelerations: [-0.06505253682638361, 0.8466679429482232, 0.11177369394743028, 0.015736713483638018, -0.9475430644471359, -0.05305393359237662]
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 190188667
    - 
      positions: [0.016760279752369364, -1.3640757878033574, -1.6804351125118788, 0.041761631069837865, 1.4812011232755613, 0.007927559399509204]
      velocities: [0.01331887794185871, -0.17253987063832876, -0.022872492625269324, -0.0030866474099697713, 0.1931845703556464, 0.010793788430771754]
      accelerations: [-0.0018174124485574599, 0.05624313737419103, 0.003610819738076645, 0.005901809596986788, -0.059404763054666826, -0.004247407067867937]
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 257998734
    - 
      positions: [0.01717530135735601, -1.3694268762976924, -1.681147449300795, 0.041669694255779495, 1.4871952420835808, 0.008261749498963473]
      velocities: [0.01218552058486485, -0.15711422782737222, -0.020915042736081565, -0.0026993725790521424, 0.17599435124732166, 0.009812212875734664]
      accelerations: [-0.06340016770936795, 0.8174511974609844, 0.10881908638072126, 0.014044592763196506, -0.9156827816482952, -0.05105206114002861]
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 289264811
    - 
      positions: [0.017590322962342655, -1.3747779647920275, -1.6818597860897109, 0.041577757441721125, 1.4931893608916003, 0.008595939598417743]
      velocities: [0.009634921437206413, -0.12422803205176897, -0.016537233039368035, -0.002134356308192497, 0.1391562827251084, 0.007758380081050698]
      accelerations: [-0.06633017694348771, 0.8552293239792368, 0.11384810979579897, 0.014693657079153094, -0.9580006350969896, -0.053411408377828214]
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 326663650
    - 
      positions: [0.0180053445673293, -1.3801290532863626, -1.6825721228786268, 0.04148582062766276, 1.49918347969962, 0.008930129697872012]
      velocities: [0.005970096695268457, -0.07697554862751388, -0.010246983430083996, -0.0013225134864991368, 0.08622555658989339, 0.004807333363783717]
      accelerations: [-0.054742851883661604, 0.7058279408639203, 0.09395980078714693, 0.012126798542968314, -0.7906459667107305, -0.04408088373134196]
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 377445329
    - 
      positions: [0.01842036617231595, -1.3854801417806974, -1.6832844596675427, 0.04139388381360439, 1.5051775985076394, 0.009264319797326283]
      velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
      accelerations: [-0.06840259322905058, 0.8819500604611046, 0.11740517367974304, 0.01515274486737107, -0.9879323525934214, -0.05508019139119717]
      effort: []
      time_from_start: 
        secs: 3
        nsecs: 487602828
multi_dof_joint_trajectory: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: ''
  joint_names: []
  points: []

---

I have really no idea where the problem lies. Sometimes the velocity is ok, sometimes not. The problem comes before Moveit! calls the controller with the trajectory, but I don't know where it can be.

It seems that my problem is very similar to https://github.com/ros-planning/moveit_core/issues/167 but this one has already been fixed.

I hope it could be an error I made on the configuration, but in this case, why is it working well on indigo, and not well at all on kinetic ?

Let me know if you and if you have any clue of how to fix this.
Any help would be appreciated
Thanks

[rbbg]

Hi @renarded,

I would not be surprised if this is caused by the planner rather than the time parameterization. Can you please check if Indigo and Kinetic are indeed using the same planner?

Some changes has been made in the default planner selection, and I have seen some planners do the back-and-forth thing that seems to be happening (judging from your plots).

[renarded]

Hi @rbbg,

Thanks for your very fast reply.

The default planner I used on Indigo was LBKPIECE1 if I remember well. I also tried with RRTConnect on Indigo and everything worked perfectly.

With RRTConnect on kinetic I have this problem. I also tried all the 15 or 20 other available planners, all give the same problem.

[rbbg]

The velocities you posted, even for the 'bad' trajectories, look pretty typical for the default IPTP time parameterization, so I would still suspect something to go wrong during planning.

What robot are you planning with? And are there any obstacles?

[renarded]

Yes I think IPTP has nothing to do with that, the problems comes from before, during the planning.

There are no obstacles in the planning scene, and my custom robot has 6 revolute joints, with nothing inside the links. The URDF I've put here is very basic. Also when I use the same planner on Indigo, or on Kinetic, it's working fine on Indigo but not on Kinetic...

I've been trying to search a solution for days, and I really don't have any clue now.

[v4hn]

MoveIt does position only planning. The time parameterization is added afterwards through IPTP.

Why do you think this could be a problem with the planning?

[renarded]

Oh, so if I understand well : 1. Moveit calculates the joints from whatever is asked (position, pose, etc), and 2. Moveit uses IPTP to add velocities and acceleration (with collision constraints and optional cartesian path constraints) ? 3. And nothing else ?

If this statement is correct it seems the problem I have is coming from IPTP (finally coming back to it !). Or it could come from a bad configuration made by myself. I would prefer the second option, but I can't find any way to solve this problem with my current URDF & SRDF, and the strange thing is : everything works perfectly with Ros Indigo.

Is there something I could do with IPTP to make my trajectories better ? Is there something in IPTP that could cause those weird trajectories, even with no constraints and no collision ?

Also let me know if you encountered some similar problems on other robotic arms.

(ps : thanks for all the fast replies :))

[rbbg]

@v4hn The reason I suspect planning is twofold. For one, I do not think there is a difference in IPTP between kinetic and indigo, but I could be mistaken.

In addition, I've done have inspected IPTP (and other parameterization algorithms) a number of times (often on an identical trajectory) and usually when I saw IPTP doing something like above, there were some unnecessary changes of direction or slight jumps in the path (on one of the axis, but they are all dependent). Unfortunately I don't really have any data to support my suspicion, but I thought I'd just share where it came from.

@renarded what cubic interpolator (I assume it's python?) did you use for plotting the trajectories? Would you mind sharing the code?

One other thought; an easy way to be sure that this problem is not related to some system setting is to test using the Indigo and Kinetic docker images

[renarded]

For this test I tried with the cubic interpolator I found from the Universal robot package which is correct and stable.

Here I've plotted the position, velocity and acceleration of the trajectory I logged on my first comment ([INFO] [1484570660.822931]: joint_trajectory:) :

This is the data from the Moveit! trajectory (directly received by the controller), no interpolation has been done by myself. You can clearly see that the acceleration is going crazy, which makes the velocity not correct (or the velocity makes the acceleration not correct, I don't really know which way Moveit! is handling this).

I hope this can help ! I don't really have any idea of where, how and why this is happening, but one thing is sure : the trajectory I get from Moveit! is not correct.

@rbbg When I have time I will try to run this test on docker and post the result here. However I've already had the same issue on different systems : amd64 Ubuntu 16.04, and armhf Ubuntu mate 16.04

[gavanderhoorn]

@renarded wrote:

For this test I tried with the cubic interpolator I found from the Universal robot package which is correct and stable.

I wouldn't necessarily assume that to be true. See ros-industrial/universal_robot#207 fi.

[renarded]

@gavanderhoorn ok thanks, that's a useful info ! I haven't seen this issue before, I might have spoken too hasty.

However the last plot with the "raw data" from Moveit! trajectory hasn't been interpolated, so the problem doesn't come from the controller side.

[rbbg]

It seems that there are a number of problems intertwined here.

The semantics of the jointTrajectory Message seem to be unclear so perhaps it's a good idea to clarify that first. The documentation is sparse, but I found the best hint in the pr2_controllers tutorial:

For each point in the trajectory, put the desired joint state into positions and velocities. The controller will interpolate between these points using cubic splines. You may optionally fill in accelerations if you wish for the controller to use quintic splines. The controller will attempt to follow whatever trajectory you specify, so it is your responsibility to ensure that the desired trajectory is smooth.

Consequently, MoveIt! is responsible for ensuring that the trajectory that is being planned will satisfy the specfied constraints _including_ the velocities that will be attained between waypoints (using cubic / quintic interpolation). If this is satisfied by both MoveIt! and the controller, the controller is not required to be aware of the limits that are set by the user. This also means that if the waypoints are set in a faulty manner, the controller will be able to violate the user-set constraints. Note that when accelerations are specified ur_modern_driver does not keep his end of the deal by only doing cubic instead of quintic interpolation.

first problem:

• The current MoveIt! time parameterization approach (IPTP) does not satisfy these constraints _between waypoints_

This is what issue #160 is all about. The violations are mostly on the acceleration limits, but velocity limits can be violated as well. On average though, the velocity profiles of IPTP look okay.

On the plots @renarded posted, there seems to be something going on with the velocities as well, which leads me to believe that this _might_ be another problem, especially since IPTP hasn't changed between kinetic and indigo.

second problem:

• MoveIt! seems to generate jerkier paths on Kinetic than on Indigo.

I have also heard at least one other report of someone who was quite familiar with MoveIt! on indigo, reporting that he felt the trajectories were not as smooth in kinetic. What might be causing this I currently have no clue but I feel _this_ is the bigger problem here.

I might be completely off the mark though :)

[jettan]

Hi, I'm glad somebody made an issue for this.
I can also confirm the second problem @rbbg mentioned. For the same moveit_configs that used to generate smooth trajectories on indigo, moveit in kinetic seems to scholastically output smooth/jerky trajectories.

I'd like to investigate further to find out the real problem, but if anybody has pointers where to start that'd be very welcome.

[jettan]

Alright I've tracked the problem down to OMPL after inspecting the diff between latest kinetic-devel and indigo-devel and seeing almost all of it were just code cleaning.
Since the current ros-kinetic-ompl is running OMPL v1.2.1 and ros-indigo-ompl is on 1.0.3094 I started suspecting the ompl version updates as it was really hard to find a changelog (I still don't know what exactly changed between the two versions except there has been more planners added in).

I then compiled ompl-1.0.3094 from source and applied some fixed for new dependencies of flann to lz4.
Recompile moveit and did some planning... all of the trajectories are 'normal' again like you'd expect them to be -- no jerky trajectories.

So I think we should move the discussion into the ompl repository instead.

[rbbg]

I discussed earlier with @gavanderhoorn that (as it is reported to affect all planners) it might be related to the PathSimplifier. Judging from the ompl release notes there have been at least _some_ changes to the PathSimplifier between these versions.

[jettan]

I created an issue in the ompl bitbucket repo

[mamoll]

Since the issue seems to be in OMPL itself, let's continue the discussion there. I have added a comment there with ideas on what might be happening.

[gavanderhoorn]

@ros-planning/moveit-developers: should this issue receive more attention? Unfortunately I don't have the bandwidth right now to do something about it, but some of the driver components under ros-industrial (cross-refs above this comment) are getting complaints that would seem to be at least related to the OMPL issue.

[v4hn]

should this issue receive more attention?

Yes.

I don't have the bandwidth right now to do something about it

Same here.

[rhaschke]

@gavanderhoorn This issue was handed over to OMPL and you already asked there for the current status. What can we do more?

[gavanderhoorn]

As developers of a tool with a large userbase and strong dependency on / ties with OMPL we could maybe try to emphasise the fact that this is a serious issue for MoveIt and its (perceived) utility in Kinetic and newer versions of ROS.

Commenting on the issue on BitBucket and / or discussing this with @mamoll in the next maintainer meeting would also be good steps, I believe.

[mamoll]

In the Melodic release of OMPL I will disable the code that finds a better goal if we cannot get this resolved.

[v4hn]

In the Melodic release of OMPL I will disable the code that finds a better goal if we cannot get this resolved.

If this really solves the problem (I do not know whether it does),
could you maybe just add an option to disable this step in melodic release and a backport to kinetic?
I guess ROS-I has more interest in kinetic than in melodic right now...

[jettan]

As far as I know (and I have frozen OMPL ever since 1+ year ago on version 1.2.1), I have not encountered this weird behavior after applying the steps commented here: https://bitbucket.org/ompl/ompl/issues/309/performance-regression-going-from-ompl#comment-33827336

[gavanderhoorn]

@v4hn wrote:

could you maybe just add an option to disable this step in melodic release and a backport to kinetic?

getting this into Kinetic would certainly be good, yes.

I guess ROS-I has more interest in kinetic than in melodic right now...

This issue affects many more users of MoveIt, also those outside ROS-Industrial. I would actually assume there are more non-ros-i users of MoveIt than the other way around.

[mamoll]

Good news: the bug appears to have been fixed, thanks to @BryceStevenWilley and @zkingston! In a nutshell, the order of reducing vertices along the path and getting better goal states was wrong. This resulted in paths that would have many more states than the original path. The gory details can be found here: https://bitbucket.org/ompl/ompl/commits/d4e24d22b75418f0f76676f5527dd6eb23312e25#chg-src/ompl/geometric/src/PathSimplifier.cpp. I will backport this fix for Kinetic.

[renarded]

Great news !

This resulted in paths that would have many more states than the original path

This seems to be exactly what was the problem when I first posted this issue.

Thanks for the backport on Kinetic, when this is ready I'll make some tests with the same config as my initial post, and I will post the result here.

[hangkaiyu]

I am having the same issue here with a Barrett WAM running MoveIt! with ROS Kinetic on Ubuntu 16.04. 2 (wrist_pitch and wrist_yaw) of the 7 joints on WAM are randomly jerking(90% of time) when executing a planned trajectory, especially during the second half of the trajectories.

I have implemented the ROS hardware interface by myself and am using the velocity_controllers/JointTrajectoryController to interface with MoveIt!

I am so glad to see that this bug has been found and a fix for it will come out. Please leave a message here when the backport fix will be available, I can't wait to fix it in my system to make WAM move like a normal robot! Thanks.

[asmorgan24]

I am glad to see that this issue is being looked into. I tried to find a work-around over a year ago and had to scrap that approach all together. I see there are some people currently interested in the work-around, do you happen to have an ETA when a fix is available for Kinetic? @mamoll

[BryceStevenWilley]

@asmorgan24 As of the 1.2.3 tag on the kinetic-devel branch of OMPL (https://bitbucket.org/ompl/ompl/commits/tag/1.2.3), these changes are available from OMPL source, and the correlated fix in MoveIt was merged with #869. If you can, you should be able to build OMPL and MoveIt from source (instructions for OMPL here, use the Bitbucket repo instead of the github for the moment, and for MoveIt here), otherwise you'll have to wait for the next Kinetic Release of OMPL.

[hangkaiyu]

@BryceStevenWilley Do you know approximately when the next Kinetic Release will be available?

[mamoll]

@hangkaiyu, hopefully I'll get this pushed out this week.

[hangkaiyu]

@mamoll Great, looking forward to it!

[mamoll]

Done: https://github.com/ros/rosdistro/pull/17747
Let me know if this fixes things.

[hangkaiyu]

I have tried the version 1.2.3-0 and the problem still exists. Here is what I did:

1. Remove old ompl installation by:
   sudo apt remove ros-kinetic-ompl
2. Get the source code of ompl from github repo ros-gbp/ompl-replease:
   git clone https://github.com/ros-gbp/ompl-release.git
3. Checked out the branch release/kinetic/ompl
4. Followed the README to build and install it
5. Followed http://moveit.ros.org/install/source/ to install MoveIt! from source
6. Run my test

When I use the motion planner in rviz to plan and execute, the trajectory is still very jerky for joint_5, and looks like it happens only to joint_5. Below is snapshot of the joint velocity command, it's been going up and down around 0 and never ends until I kill the program.

FYI, I am using a hardware_interface with velocity_controllers/JointTrajectoryController implemented by myself.

[BryceStevenWilley]

1. Run my test

@hangkaiyu can you explain this step further? You mention above that you are using the Barrett WAM, but what trajectory are you executing and what sort of environment?

Below is snapshot of the joint velocity command, it's been going up and down around 0 and never ends until I kill the program.

This fix changed the trajectories that MoveIt was creating to execute, not necessarily the controllers themselves. Your comment here suggests that the issues lies somewhere in your velocity controller, as if the robot is not currently executing a trajectory, then the MoveIt/OMPL planning is not executed. Could you give a graph of what the trajectory looks like for each joint position? Something similar to this image.

[hangkaiyu]

@BryceStevenWilley

what trajectory are you executing and what sort of environment?

It was pretty simple -- I just set random goals for WAM to move around using MoveIt! in an empty environment.

Your comment here suggests that the issues lies somewhere in your velocity controller

As you suggested, I just did some tests on the velocity controller of our WAM. Yes you are right, the velocity controller of WAM has some problems for joint 4 and 5 -- as long as I send random velocity commands (between 0.4 and 0.6 rad/s) to the controller, those two joints will start jerking and the velocities will bounce between positive and negative values.

In my test, I tried to send random velocity control commands at between 5Hz and 500Hz, and it was always the same problem, and it won't happen if I send constant velocity commands. so it is obvious that the WAM controller cannot handle sudden velocity changes for those two joints.

Thanks for your help! I'll contact Barrett to see how this can be solved.

[v4hn]

It would be nice if somebody who experienced the problem could verify that the current MoveIt kinetic-devel together with OMPL 1.2.3-0 resolves this problem.

[jettan]

Hi, I will test this today.

On Tue, May 15, 2018 at 1:29 Michael Görner [e-mail]
wrote:

It would be nice if somebody who experienced the problem could verify that
the current MoveIt kinetic-devel together with OMPL 1.2.3-0 resolves this
problem.

—
You are receiving this because you commented.
Reply to this email directly, view it on GitHub
https://github.com/ros-planning/moveit/issues/416#issuecomment-388879373,
or mute the thread
https://github.com/notifications/unsubscribe-auth/ABZb365Vpe4b_wDX2mp4O3DNvUqlflxvks5tybDVgaJpZM4LkjlM
.

[jettan]

Unfortunately we are at ROS Lunar and there is no release yet as of now. There seems to be compile problems with the latest moveit on kinetic-devel in combination with the latest OMPL as found from github (EuclideanProjection has been deprecated). So I cannot really confirm the fix at this moment.

[zkingston]

@jettan The latest OMPL on github is the candidate for OMPL 1.4.0. You should try OMPL 1.2.3-0, which you can get from here: https://github.com/ros-gbp/ompl-release/tree/release/kinetic/ompl.

[jettan]

Thanks, I’ll try again tomorrow.

[jettan]

I tested today and have verified that using the current MoveIt kinetic-devel together with OMPL 1.2.3-0 resolves this problem.

[v4hn]

Thanks a lot for the patches and the support from the OMPL crew! :+1:
Closing.

[hangkaiyu]

Thanks so much for the OMPL team's great effort! Really appreciate your brilliant developments and quick responses!

[renarded]

Thanks a lot !

I've also tested (I know, a little bit late) with the configuration shown in the first message of this thread, and the explained problem is definitively gone.