While Planning with Approximated Constraint Manifolds, it still takes time(~10s) to compute constraints path

asked 2021-06-16 11:39:06 -0600

artemiialessandrini gravatar image

updated 2021-06-16 11:57:15 -0600

I'm using Planning with Constraints Manifold to add Orientation Constraints to
keep grasped objects upright.

To generate database, the following parameters, written in constraints.yaml file used:

  name: path_constraints
  constraints:
  - type: orientation
    frame_id: base
    link_name: right_hand
    orientation: [1.5707963, 0, 1.57079632] #RPY
    tolerances: [0.4, 6.28318531, 0.4]
    weight: 0.8

The database seems to be successfully generated. While loading it in Moveit!, the following output received:

[ INFO] [1623860951.925316018]: Initializing OMPL interface using ROS parameters
[ INFO] [1623860951.967107499]: Loading constrained space approximations from '/home/artemii/sawyer_ws/src/sawyer_vending/sawyer_moveit_config/constraint_approximations_database/'...
[ INFO] [1623860951.967148969]: Loading constraint approximation of type 'JointModel' for group 'right_arm' from 'right_arm_2021-06-16T16:22:43.820172.ompldb'...
[ INFO] [1623860951.971008453]: Loaded 10119 states (10000 milestones) and 38 connections (0.0 per state) for constraint named 'path_constraints'. Explicit motions included.
[ INFO] [1623860951.971034737]: Done loading constrained space approximations.
[ INFO] [1623860951.971114476]: right_arm
JointModel
1
10000
right_arm_2021-06-16T16:22:43.820172.ompldb
name: path_constraints
joint_constraints[]
position_constraints[]
orientation_constraints[]
  orientation_constraints[0]: 
    header: 
      seq: 0
      stamp: 0.000000000
      frame_id: base
    orientation: 
      x: 0.5
      y: 0.5
      z: 0.5
      w: 0.5
        link_name: right_hand
        absolute_x_axis_tolerance: 0.4
        absolute_y_axis_tolerance: 6.28319
        absolute_z_axis_tolerance: 0.4
        weight: 0.8
    visibility_constraints[]

Yet, Constraints Planning takes quite a lot of time, and I have no idea how to debug/visualize it.
Param enforce_joint_model_state_space is already set to true.

Also, I'm using right_hand link to calculate constraints, since there's an issue with Schunk gripper's links, created automatically from InteraSDK! (see the image).

For more info, attaching generate_state_database.launch console output and
Moveit! Contraints console output.

Best Regards,
Artemii

image description

edit retag flag offensive close merge delete

Comments

hi, did you manage to use the constraint database for planning?

xibeisiber gravatar image xibeisiber  ( 2021-11-03 09:34:48 -0600 )edit