While Planning with Approximated Constraint Manifolds, it still takes time(~10s) to compute constraints path
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
hi, did you manage to use the constraint database for planning?