ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org

Plan multiple waypoints trajectory with contraints between poses N and N+1 only

Hi there,
I am trying to get a robot arm (Panda from the moveit tutorials) to grasp a cup in Gazebo, using ROS melodic on Ubuntu bionic.
For the grasping part I just get the pose of the cup from Gazebo, and send the endeffector in front of that cup. I finally generate a dynamic fixed link between both objects.
My problem is that I want to move the cup from position A to position B without spilling its content in the trajectory followed. Whenever I set constraints as follows:

    upright_constraints = Constraints()
    upright_constraints.name = "upright"
    orientation_constraint = OrientationConstraint()
    orientation_constraint.header = pose.header
    orientation_constraint.link_name = self.arm.get_end_effector_link()
    orientation_constraint.orientation = pose.pose.orientation
    orientation_constraint.absolute_x_axis_tolerance = 0.5
    orientation_constraint.absolute_y_axis_tolerance = 0.5
    orientation_constraint.absolute_z_axis_tolerance = 3.14 # allow free rotation around this axis
    orientation_constraint.weight = 1

My planner fails to come up with a plan. I tried extending the planning time to 5 minutes, with no success.
I am now thinking that the problem is that when the planner plans for a trajectory leading to the cup it does not know about what needs to be done next. So the arm might arrive in a joint configuration that makes it impossible to move the cup to point B without having to flip it over at least once.
I know that it is possible to send multiple Cartesian pose goals to the planner so it can preprocess all of them and find the optimal trajectory, but how to tell it that starting from the pose in front of the cup, it should include the contraints? If that is not implemented yet, where could it be integrated in the code?

Sounds like a cool problem!

Which planner are you using exactly?

MoveIt has several great options. I have use TRAC Lab’s IK solver to some pretty great success in the past (not doing exatly what you’re doing, but worth a shot):

https://ros-planning.github.io/moveit_tutorials/doc/trac_ik/trac_ik_tutorial.html

More information about what you are using would be great!

This type of troubleshooting question is best asked on answers.ros.org

We’ve had trouble with constrained motion plans, too. I would recommend making a Cartesian plan, instead. That will also keep the cup upright since it’s a linear interpolation (and it works, the last time I checked).

2 Likes

I recommend using TRAC_IK or bio_ik for better results too.

On bio_ik you can get iks with further constraints and they work (or at least they did on my use-case).

I tried out TRAC_IK, but got similar results. I think OMPL with RTTConnect is the problem but couldn’t figure out how to change it in the parameters. Whatever I do RTTConnect is the one doing the planning.
I basically want to move a cube from front to rear (modeled as a cube in moveit but it is a cup). I think the most confusing thing here are these lines:

    orientation_constraint.absolute_x_axis_tolerance = 0.5
    orientation_constraint.absolute_y_axis_tolerance = 0.5
    orientation_constraint.absolute_z_axis_tolerance = 3.14

There is zero documentation on how those values should be set. What do they represent? How can one translate them into “You can turn the endeffector around world’s z axis as much as you want but around the two other axis it should be limited to few degrees to not spill the cup”. I tried all combinations of values, nothing seems to work well. Most of the times it doesn’t find a trajectory but in few cases it does and here is what it looks like:

Here are my constraints:

name: "upright"
joint_constraints: []
position_constraints: []
orientation_constraints: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 349
        nsecs: 157000000
      frame_id: "world"
    orientation: 
      x: -0.70761630062
      y: 0.000350377689552
      z: -0.706596714945
      w: 0.000361608916337
    link_name: "panda_link8"
    absolute_x_axis_tolerance: 3.14
    absolute_y_axis_tolerance: 0.1
    absolute_z_axis_tolerance: 0.1
    weight: 1.0
visibility_constraints: []

i don’t know why for those value (x 3.14, y 0.1, z 0.1 it managed to find a path, totally unintuitive)
Here is the log from RTT

[/move_group INFO 1563826183.386238577, 352.138000000]: panda_arm: Allocating specialized state sampler for state space
[/move_group INFO 1563826183.386365074, 352.138000000]: RRTConnect: Starting planning with 1 states already in datastructure
[/move_group INFO 1563826191.847614911, 360.447000000]: RRTConnect: Created 8 states (2 start + 6 goal)
[/move_group INFO 1563826191.847754873, 360.447000000]: Solution found in 8.462048 seconds
[/move_group INFO 1563826192.703497689, 361.262000000]: panda_arm: Allocating specialized state sampler for state space
[/move_group INFO 1563826193.788332731, 362.328000000]: panda_arm: Allocating specialized state sampler for state space
[/move_group INFO 1563826198.994204393, 367.406000000]: SimpleSetup: Path simplification took 7.146335 seconds and changed from 4 to 4 states

And my ros params for the move group:

allow_trajectory_execution: true
controller_list:
- action_ns: follow_joint_trajectory
  default: true
  joints: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6,
    panda_joint7]
  name: panda_arm_controller
  type: FollowJointTrajectory
hand:
  planner_configs: [SBLkConfigDefault, ESTkConfigDefault, LBKPIECEkConfigDefault,
    BKPIECEkConfigDefault, KPIECEkConfigDefault, RRTkConfigDefault, RRTConnectkConfigDefault,
    RRTstarkConfigDefault, TRRTkConfigDefault, PRMkConfigDefault, PRMstarkConfigDefault,
    FMTkConfigDefault, BFMTkConfigDefault, PDSTkConfigDefault, STRIDEkConfigDefault,
    BiTRRTkConfigDefault, LBTRRTkConfigDefault, BiESTkConfigDefault, ProjESTkConfigDefault,
    LazyPRMkConfigDefault, LazyPRMstarkConfigDefault, SPARSkConfigDefault, SPARStwokConfigDefault,
    TrajOptDefault]
jiggle_fraction: 0.05
max_range: 5.0
max_safe_path_cost: 1
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_manage_controllers: true
octomap_frame: world
octomap_resolution: 0.05
ompl: {display_random_valid_states: false, link_for_exploration_tree: '', maximum_waypoint_distance: 0.0,
  minimum_waypoint_count: 10, simplify_solutions: true}
panda_arm:
  planner_configs: [SBLkConfigDefault, ESTkConfigDefault, LBKPIECEkConfigDefault,
    BKPIECEkConfigDefault, KPIECEkConfigDefault, RRTkConfigDefault, RRTConnectkConfigDefault,
    RRTstarkConfigDefault, TRRTkConfigDefault, PRMkConfigDefault, PRMstarkConfigDefault,
    FMTkConfigDefault, BFMTkConfigDefault, PDSTkConfigDefault, STRIDEkConfigDefault,
    BiTRRTkConfigDefault, LBTRRTkConfigDefault, BiESTkConfigDefault, ProjESTkConfigDefault,
    LazyPRMkConfigDefault, LazyPRMstarkConfigDefault, SPARSkConfigDefault, SPARStwokConfigDefault,
    TrajOptDefault]
panda_arm_hand:
  planner_configs: [SBLkConfigDefault, ESTkConfigDefault, LBKPIECEkConfigDefault,
    BKPIECEkConfigDefault, KPIECEkConfigDefault, RRTkConfigDefault, RRTConnectkConfigDefault,
    RRTstarkConfigDefault, TRRTkConfigDefault, PRMkConfigDefault, PRMstarkConfigDefault,
    FMTkConfigDefault, BFMTkConfigDefault, PDSTkConfigDefault, STRIDEkConfigDefault,
    BiTRRTkConfigDefault, LBTRRTkConfigDefault, BiESTkConfigDefault, ProjESTkConfigDefault,
    LazyPRMkConfigDefault, LazyPRMstarkConfigDefault, SPARSkConfigDefault, SPARStwokConfigDefault,
    TrajOptDefault]
plan_execution: {max_replan_attempts: 5, record_trajectory_state_frequency: 10.0}
planner_configs:
  BFMTkConfigDefault: {balanced: 0, cache_cc: 1, extended_fmt: 1, heuristics: 1, nearest_k: 1,
    num_samples: 1000, optimality: 1, radius_multiplier: 1.0, type: 'geometric::BFMT'}
  BKPIECEkConfigDefault: {border_fraction: 0.9, failed_expansion_score_factor: 0.5,
    min_valid_path_fraction: 0.5, range: 0.0, type: 'geometric::BKPIECE'}
  BiESTkConfigDefault: {range: 0.0, type: 'geometric::BiEST'}
  BiTRRTkConfigDefault: {cost_threshold: 1e300, frountier_node_ratio: 0.1, frountier_threshold: 0.0,
    init_temperature: 100, range: 0.0, temp_change_factor: 0.1, type: 'geometric::BiTRRT'}
  ESTkConfigDefault: {goal_bias: 0.05, range: 0.0, type: 'geometric::EST'}
  FMTkConfigDefault: {cache_cc: 1, extended_fmt: 1, heuristics: 0, nearest_k: 1, num_samples: 1000,
    radius_multiplier: 1.1, type: 'geometric::FMT'}
  KPIECEkConfigDefault: {border_fraction: 0.9, failed_expansion_score_factor: 0.5,
    goal_bias: 0.05, min_valid_path_fraction: 0.5, range: 0.0, type: 'geometric::KPIECE'}
  LBKPIECEkConfigDefault: {border_fraction: 0.9, min_valid_path_fraction: 0.5, range: 0.0,
    type: 'geometric::LBKPIECE'}
  LBTRRTkConfigDefault: {epsilon: 0.4, goal_bias: 0.05, range: 0.0, type: 'geometric::LBTRRT'}
  LazyPRMkConfigDefault: {range: 0.0, type: 'geometric::LazyPRM'}
  LazyPRMstarkConfigDefault: {type: 'geometric::LazyPRMstar'}
  PDSTkConfigDefault: {type: 'geometric::PDST'}
  PRMkConfigDefault: {max_nearest_neighbors: 10, type: 'geometric::PRM'}
  PRMstarkConfigDefault: {type: 'geometric::PRMstar'}
  ProjESTkConfigDefault: {goal_bias: 0.05, range: 0.0, type: 'geometric::ProjEST'}
  RRTConnectkConfigDefault: {range: 0.1, type: 'geometric::RRTConnect'}
  RRTkConfigDefault: {goal_bias: 0.05, range: 0.0, type: 'geometric::RRT'}
  RRTstarkConfigDefault: {delay_collision_checking: 1, goal_bias: 0.05, range: 0.0,
    type: 'geometric::RRTstar'}
  SBLkConfigDefault: {range: 0.0, type: 'geometric::SBL'}
  SPARSkConfigDefault: {dense_delta_fraction: 0.001, max_failures: 1000, sparse_delta_fraction: 0.25,
    stretch_factor: 3.0, type: 'geometric::SPARS'}
  SPARStwokConfigDefault: {dense_delta_fraction: 0.001, max_failures: 5000, sparse_delta_fraction: 0.25,
    stretch_factor: 3.0, type: 'geometric::SPARStwo'}
  STRIDEkConfigDefault: {degree: 16, estimated_dimension: 0.0, goal_bias: 0.05, max_degree: 18,
    max_pts_per_leaf: 6, min_degree: 12, min_valid_path_fraction: 0.2, range: 0.0,
    type: 'geometric::STRIDE', use_projected_distance: 0}
  TRRTkConfigDefault: {frountierNodeRatio: 0.1, frountier_threshold: 0.0, goal_bias: 0.05,
    init_temperature: 10e-6, k_constant: 0.0, max_states_failed: 10, min_temperature: 10e-10,
    range: 0.0, temp_change_factor: 2.0, type: 'geometric::TRRT'}
  TrajOptDefault: {type: 'geometric::TrajOpt'}
planning_plugin: ompl_interface/OMPLPlanner
planning_scene_monitor: {publish_geometry_updates: true, publish_planning_scene: true,
  publish_planning_scene_hz: 4.0, publish_state_updates: true, publish_transforms_updates: true}
request_adapters: default_planner_request_adapters/AddTimeParameterization            default_planner_request_adapters/FixWorkspaceBounds            default_planner_request_adapters/FixStartStateBounds            default_planner_request_adapters/FixStartStateCollision            default_planner_request_adapters/FixStartStatePathConstraints
sense_for_plan: {discard_overlapping_cost_sources: 0.8, max_cost_sources: 100, max_look_attempts: 3,
  max_safe_path_cost: 0.01}
sensors:
- {filtered_cloud_topic: filtered_cloud, max_range: 5.0, max_update_rate: 1.0, padding_offset: 0.1,
  padding_scale: 1.0, point_cloud_topic: /camera/depth/points2, point_subsample: 1,
  sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater}
start_state_max_bounds_error: 0.1
trajectory_execution: {allowed_execution_duration_scaling: 1.2, allowed_goal_duration_margin: 0.5,
  allowed_start_tolerance: 0.01, execution_duration_monitoring: true, execution_velocity_scaling: 1.0,
  wait_for_trajectory_completion: true}

Finally, the resulting planned trajectory that you see in the GIF:

joint_trajectory: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "world"
  joint_names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6,
  panda_joint7]
  points: 
    - 
      positions: [-2.213983065136418, -1.2906379420367688, 2.3273649916983246, -1.9678602917554295, -2.650969141769095, 2.0039492641542935, -2.476349470124318]
      velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
      accelerations: [1.9632485719663737, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
      effort: []
      time_from_start: 
        secs: 0
        nsecs:         0
    - 
      positions: [-1.9027151256478418, -1.5880583544291564, 2.1292902456872462, -2.154423175484103, -2.4068879685174527, 2.2317096667180625, -2.152342548895806]
      velocities: [0.4298869667738717, -0.4556318780073839, -0.19791464189962368, -0.34017229377693714, 0.30441774361787766, 0.3997345551847432, 0.5112288002771675]
      accelerations: [-0.48222665458307257, 0.28468497464592474, 0.6037223941527191, -0.0347912472704278, -0.5063849797233159, -0.018574417137113254, -0.2517847982946746]
      effort: []
      time_from_start: 
        secs: 0
        nsecs: 563111696
    - 
      positions: [-1.7626767727462216, -1.7627999782562256, 2.1091842473515934, -2.313631894582778, -2.32688859444776, 2.4118842614471276, -1.9484171891276754]
      velocities: [0.7725408157720929, 0.5650829187666142, -0.8316923201322326, -0.27305036858669096, -0.059530639605950175, -0.061749502808752355, -0.07950400818817896]
      accelerations: [0.9210476893316034, 1.8759505680749602, -1.5582839960917374, 0.15033970271106387, -0.4647774006985861, -0.9036767076602864, -1.0418225664459562]
      effort: []
      time_from_start: 
        secs: 1
        nsecs:  19248097
    - 
      positions: [0.17565969203819773, 0.6063682213183783, -0.42601548374175996, -2.622155852089251, -2.787875987452324, 1.6001150559915056, -2.8973000049591064]
      velocities: [0.7341210653027398, 0.17826847159587578, -1.2539442120327753, -0.10109952836094367, 1.1382694873282135, -0.1157564282770443, 0.8649658386396004]
      accelerations: [-0.5642507779739733, -1.494725563291784, 0.40907869952418036, 0.1074461840484824, 1.6041465406503854, 0.4509343400663588, 1.6470621546324373]
      effort: []
      time_from_start: 
        secs: 2
        nsecs: 584857169
    - 
      positions: [0.6375933836556927, -1.715076928250512, -2.209330963234938, -2.632463081337065, 2.3718884042035207, 2.1760768007872247, 1.7908894452450517]
      velocities: [-0.9724148421082567, -0.2786311262121256, -0.24537416592234756, -0.26398524373559246, 1.44979787515851, 0.2962246413803101, 1.290045413951348]
      accelerations: [-2.265003772396396, 1.6538319407711919, 1.2114483097018662, -0.487528778117418, -2.11169503151197, 0.017397240400491905, -1.970018757212809]
      effort: []
      time_from_start: 
        secs: 4
        nsecs: 591778392
    - 
      positions: [0.3834534679918675, -1.6450327699773575, -2.162845633190281, -2.693554186091228, 2.4102852338042835, 2.2117687437513056, 1.8194092168206337]
      velocities: [-1.1057388901973995, 0.8980283437714662, 0.4753569299354654, -0.2870429342055714, -1.1406946326917613, 0.03847686947447697, -0.979036762608576]
      accelerations: [2.0414500089035927, 0.5700363748479903, 0.14800728500760188, 0.45017721838671665, -2.805220752606676, -0.5097313468909557, -2.3351938685388656]
      effort: []
      time_from_start: 
        secs: 4
        nsecs: 708624330
    - 
      positions: [0.3112911056336616, 0.7221461800472077, -1.0691088496407837, -2.794942188403328, -2.752961096330716, 1.7597219293004558, -2.5370129169004394]
      velocities: [-0.0822034140150121, 0.0563778643209657, 0.06241899490750913, 0.009631387551305472, -0.04317740752954591, 0.007597584700165083, -0.06108272493000144]
      accelerations: [-0.044676352708033114, -1.114056427200839, -0.47920574077546685, 0.059485517010125465, 2.5079208837789757, 0.23068742186582555, 2.091939080935357]
      effort: []
      time_from_start: 
        secs: 6
        nsecs: 686879629
    - 
      positions: [0.040632926504200945, -1.5709306382845338, -1.9747118816096683, -2.6457564442484136, 2.586289239312852, 2.2753218635486214, 1.8635968755718382]
      velocities: [-0.06009464200741561, 0.01035520782793864, 0.011239616211776271, 0.19739327656239786, 1.5482026347918063, -0.09049110448083351, 1.5112201506008778]
      accelerations: [0.11551318402015218, 1.8632784697111437, 0.7480386999626303, 0.21605893607126836, -1.6610519949815388, -0.5690891519633852, -0.9685381356141484]
      effort: []
      time_from_start: 
        secs: 8
        nsecs: 802569413
    - 
      positions: [0.042438501686269905, -1.3132541245940084, -1.8696118945480387, -2.5701083356817946, 2.719905903184296, 2.176248933172528, 2.0834573521176147]
      velocities: [0.15573369117473462, 1.3060182524842707, 0.2446091442826451, 0.43154671194466215, 0.6698886942074038, -0.3859866968381853, 1.095575846120204]
      accelerations: [1.2786570114914173, 1.7406367365035307, -1.779061494769991, 0.9268410833867544, 0.8391842806571446, 0.33435233962341, 1.3229782988651269]
      effort: []
      time_from_start: 
        secs: 9
        nsecs:  35854985
    - 
      positions: [0.11219902594267216, -0.9670137344592727, -1.8607237937442624, -2.4463513115140865, 2.896075358727299, 2.0964833643778933, 2.3702601747158014]
      velocities: [0.873628433929553, 1.1809565592328568, 1.1004671280578466, 0.18584474939185258, 0.34463552985202156, -0.13847991364238274, 0.7789881107068108]
      accelerations: [1.178829035327993, -0.6754127110579362, 2.1962502398777835, -0.730124412219956, -0.8736886942602131, 0.43191596415573785, -0.9715893021702185]
      effort: []
      time_from_start: 
        secs: 9
        nsecs: 265536186
    - 
      positions: [2.5721243622218797, 0.4890257177649022, 1.8239544685384415, -2.7311607052669737, 2.7635875995088224, 2.216330276844053, 2.8973000049591064]
      velocities: [-0.3389843527574834, -0.15634577986854664, 0.09134042429567002, -0.09032115063100243, -0.006603801070785689, 0.26163126545081683, -0.10170393076049722]
      accelerations: [-2.1410218142378263, -1.2140688997749598, -2.487405481778824, 0.09225868120801983, 0.08545097339736743, 0.2297787155636069, -0.4936390601629802]
      effort: []
      time_from_start: 
        secs: 10
        nsecs: 969641134
    - 
      positions: [-0.877656311197903, -1.4088387322556999, -1.395013293663802, -2.7531309132445787, 2.868534276039565, 2.952849269544255, 2.063620653150253]
      velocities: [-0.18185566878006365, 0.3656607890125213, 0.07141675528787894, -0.015062471017449727, -0.12287871553923543, 0.39181868032980566, -0.03125033009639877]
      accelerations: [2.365587585497385, 1.8693819964823601, 2.501366038305531, -0.001892257153520745, -0.22857419502480128, -0.07453644325804584, 0.5871570631603973]
      effort: []
      time_from_start: 
        secs: 12
        nsecs: 595747191
    - 
      positions: [2.0291215959835713, 1.7305379859744194, 2.1146928631987247, -2.780604804289588, 2.355410691567288, 3.499719251966208, 2.8080707435790555]
      velocities: [0.48970591767462196, 0.7622439596459536, 1.187568037653634, -0.04796541909131588, -0.5029115309570179, -0.32411519497819385, -1.0799080858894676]
      accelerations: [-1.4576019644450484, -1.3060107788249675, -1.0745352418659437, -0.036037040158017876, -0.22140284133649818, -0.7526844069839503, -1.7587721198077022]
      effort: []
      time_from_start: 
        secs: 14
        nsecs: 249405194
    - 
      positions: [0.6076135528310457, 1.047600603296674, 2.5762719020620928, -2.925457273155385, 1.0852035469327912, 1.711940414451464, -1.9584454381954894]
      velocities: [-0.6165430988928429, -1.194252716627932, 0.012133182669320161, -0.03121159828329711, 0.6077794068436323, 0.8155332083257167, -2.44486208665312]
      accelerations: [0.3423382862445737, -1.735253700519111, -0.5089931819581115, 0.10176166731961772, 2.757010680084333, 3.796009998742757, 0.34933227694416746]
      effort: []
      time_from_start: 
        secs: 16
        nsecs:  75656605
    - 
      positions: [0.5782180762728595, 0.9173671338573478, 2.5615014599095693, -2.9243651599452685, 1.2087484502011063, 1.8806676359567618, -2.105821501628602]
      velocities: [-0.5903974143626988, -1.001439297489445, -1.2017401732042856, 0.011134630219391362, 1.1143488737408493, 1.0951415813202787, -1.1622622505522564]
      accelerations: [-0.23550384898766588, 1.7584082128204002, -1.6892416176010765, -0.009995629238600045, -1.3828590695389533, -2.6292689939048137, 1.939526459460825]
      effort: []
      time_from_start: 
        secs: 16
        nsecs: 140303050
    - 
      positions: [-1.048180665273533, 0.9435097153804629, -2.31041435735085, -2.91232395884303, 1.9201869136667706, 0.9405181652891821, -2.2061725019131355]
      velocities: [0.7244582936623309, -0.42572663972343233, -0.2687322736018446, 0.020633624334952496, 0.14898197609297778, -0.018094033554471478, -0.07980277144771461]
      accelerations: [1.479211016066067, -0.44604260587131467, 1.9439442576103059, 0.015559562712993625, -0.1719628648984873, 0.4095606993552777, -0.035694251333323705]
      effort: []
      time_from_start: 
        secs: 18
        nsecs: 380264345
    - 
      positions: [2.611284576741554, -0.5087078888221097, 0.4447599286608907, -2.851935851106143, 1.8871289153408721, 1.5858102131257303, -2.399333740648817]
      velocities: [0.25460266036087276, -1.1640455024158167, 1.796938067613762, -0.010130115284625985, 0.8742089709884164, -0.28146486827618944, 0.42897423649013394]
      accelerations: [-3.5238882590753944, -0.5521840471575954, 0.2925004066745321, -0.08444890429679759, 1.6402085102993285, -1.220249178018743, 0.997823723987763]
      effort: []
      time_from_start: 
        secs: 20
        nsecs:  62777100
    - 
      positions: [1.7828032049631843, -1.2373076735689637, 1.4177440739510478, -2.879862919440033, 2.7664747970253045, 1.1150899906388292, -1.9155356352004123]
      velocities: [-1.5955391945713457, -0.8511702859547261, 0.271129914097039, 0.08641831957745112, -0.39734742946380897, -0.03414434405636346, 1.1621663116846397]
      accelerations: [0.10379857219388616, 0.9068501143975796, -2.4898052063634606, 0.21063949834905016, -3.1992778406551117, 1.3478934474711135, 0.27984661934862]
      effort: []
      time_from_start: 
        secs: 20
        nsecs: 560126132
    - 
      positions: [-1.588117508108596, -1.7619106708029857, -1.7074151411367808, -2.3737917953247223, -2.8973000049591064, 3.0558704580839433, 1.071492547728093]
      velocities: [-0.7542808442636779, -0.08332829097208842, -0.7002384842045246, 0.12393857299918637, 0.023619603198073325, 0.38772649316269664, 0.8279694862089577]
      accelerations: [0.6982358020068443, 0.13950677062351668, 0.6464701001569084, -0.09513533066295549, 2.3422784417910396, -0.44415540332880027, -0.47419131618127547]
      effort: []
      time_from_start: 
        secs: 22
        nsecs: 770154792
    - 
      positions: [-1.551214787164648, -1.6058494317425738, -1.67739362685047, -2.3321076703621157, 2.8625360652246057, 2.8291914864097873, 1.7431645477898299]
      velocities: [0.2252630390159866, 0.2372664267176807, -0.25773156052744706, 0.1834066222679872, 1.0923334689605548, -1.0477626596852978, 0.5059614147860833]
      accelerations: [0.356564259665598, 0.2847664235626102, -0.4639304230295883, 0.28129347045934544, -2.5949122579450985, -1.615843106162693, 0.34469883561991604]
      effort: []
      time_from_start: 
        secs: 24
        nsecs: 976988535
    - 
      positions: [-1.4936855440248016, -1.5522971308596418, -1.7475562359974126, -2.285967395970261, 2.8061302092485705, 2.564913727336774, 1.8369985064518861]
      velocities: [0.3719999536076576, 0.36143085353845145, -0.3910554132317936, 0.26479135809727183, -0.18159447747975666, -1.404286885872421, 0.5220853355597146]
      accelerations: [-0.5062795401173359, -0.34720228526413066, 1.1305470458160551, -0.6810009565776021, 1.9966289025426953, 4.820981628847374, -1.5193722889568444]
      effort: []
      time_from_start: 
        secs: 25
        nsecs: 109604269
    - 
      positions: [-1.3833536912573983, -1.4388173762145, -1.8375599978105666, -2.2213544173055118, 2.828233904270775, 2.27475866840359, 1.9567247079720698]
      velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
      accelerations: [-1.744219158232369, -1.7939838510719086, 1.4228555193252985, -1.0214565642716267, -0.3494338883869918, 4.587016350719919, -1.8927329614788317]
      effort: []
      time_from_start: 
        secs: 25
        nsecs: 465288758
multi_dof_joint_trajectory: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: ''
  joint_names: []
  points: []

Am I doing something wrong? I will give bio_ik a try too but as I said it seems to be caused by RTTConnect.

@Mehdi_Tlili Please move this to a question on answers.ros.org as @AndyZe suggested. When you do please edit your post to link to the new location so someone who finds this thread will find the followups.

https://wiki.ros.org/Support

1 Like

Done, under this link

1 Like