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
