compas-dev / compas-dev/compas_fab
IK Solution differ from motion planning solution?
- Dominant language
- Python
- Stars
- 137
- Forks
- 47
- PR merge metrics
- No merged PRs in 30d
Description
**Describe the bug**
See code snippet
I'm trying to do a path planning that constrains both end effector and joint positions (within a range)
If I try to compute path planning for the to constraint separately everything work, but if try to merge them planning solution fail with: `compas_fab.backends.ros.exceptions.RosValidationError: Error code: -1; PLANNING_FAILED`
The joint configuration target target is obtain from inverse kinematic
**To Reproduce**
```
import compas
import compas_fab
from compas.geometry import Frame
from compas_fab.backends import RosClient
import math
print(f"Compas Version:\t{compas.__version__}")
print(f"Compas_Fab Version:\t{compas_fab.__version__}")
compas.PRECISION = '24f'
FINAL_TOLLERANCE_POSITION = 0.1 / 1000
FINAL_TOLLERANCE_AXES = math.radians(0.1)
MOTION_TOLLERANCE_AXES = math.radians(2.5)
PLANNER_PARAM = {'planner_id': 'RRTConnect', 'num_planning_attempts' : 8, 'allowed_planning_time' : 5}
with RosClient() as ros:
robot = ros.load_robot()
print(robot.model)
target_frame = Frame((0.5, 0.5, 0.5), (1,0,0), (0,1,0))
target_config = robot.inverse_kinematics(target_frame)
frame_constraints = robot.constraints_from_frame(target_frame, FINAL_TOLLERANCE_POSITION, [FINAL_TOLLERANCE_AXES])
config_constraints = robot.constraints_from_configuration(target_config, [MOTION_TOLLERANCE_AXES], [MOTION_TOLLERANCE_AXES])
combo_constraints = [*frame_constraints, *config_constraints]
robot.plan_motion(frame_constraints, options=PLANNER_PARAM)
print('frame_constraints PLANNED')
robot.plan_motion(config_constraints, options=PLANNER_PARAM)
print('config_constraints PLANNED')
robot.plan_motion(combo_constraints, options=PLANNER_PARAM)
print('combo_constraints PLANNED')
print('\nEND')
```
**Expected behavior**
> Compute `combo_constraints` without failing given the fact they should be the same joint configuration as `config_constraints` and same frame as `frame_constraints`
**LOGS**
> Python script:
```
Compas Version: 1.16.0
Compas_Fab Version: 0.26.0
Robot name=ur10e_robot, Links=11, Joints=10 (6 configurable)
frame_constraints PLANNED
config_constraints PLANNED
Traceback (most recent call last):
File "/Users/lucapassarella/Documents/SUPSI workswhop/Blender/test.py", line 42, in
robot.plan_motion(combo_constraints, options=PLANNER_PARAM)
File "/usr/local/lib/python3.9/site-packages/compas_fab/robots/robot.py", line 1667, in plan_motion
trajectory = self.client.plan_motion(
File "/usr/local/lib/python3.9/site-packages/compas_fab/backends/interfaces/client.py", line 39, in plan_motion
return self.planner.plan_motion(*args, **kwargs)
File "/usr/local/lib/python3.9/site-packages/compas_fab/backends/ros/planner.py", line 49, in plan_motion
return MoveItPlanMotion(self.client)(*args, **kwargs)
File "/usr/local/lib/python3.9/site-packages/compas_fab/backends/interfaces/backend_features.py", line 91, in __call__
return self.plan_motion(robot, goal_constraints, start_configuration, group, options)
File "/usr/local/lib/python3.9/site-packages/compas_fab/backends/ros/backend_features/move_it_plan_motion.py", line 101, in plan_motion
return await_callback(self.plan_motion_async, **kwargs)
File "/usr/local/lib/python3.9/site-packages/compas/utilities/azync.py", line 136, in await_callback
raise call_results['exception']
compas_fab.backends.ros.exceptions.RosValidationError: Error code: -1; PLANNING_FAILED
```
> ROS docker:
```
ros-bridge | 2022-07-25 13:07:40+0000 [-] [INFO] [1658754460.970089]: Client connected. 1 clients total.
moveit-demo | , tolerance_above: 0.043633, tolerance_below: 0.043633
moveit-demo | [ INFO] [1658754147.216838765]: Constraint satisfied:: Joint name: 'wrist_1_joint', actual value: 0.511312, desired value: 0.494317, tolerance_above: 0.043633, tolerance_below: 0.043633
moveit-demo | [ INFO] [1658754147.216854146]: Constraint satisfied:: Joint name: 'wrist_2_joint', actual value: 1.548066, desired value: 1.570796, tolerance_above: 0.043633, tolerance_below: 0.043633
moveit-demo | [ INFO] [1658754147.216864936]: Constraint satisfied:: Joint name: 'wrist_3_joint', actual value: -5.768970, desired value: -5.746633, tolerance_above: 0.043633, tolerance_below: 0.043633
moveit-demo | [ INFO] [1658754147.216877073]: Position constraint violated on link 'tool0'. Desired: 0.500000, 0.500000, 0.500000, current: 0.496711, 0.521341, 0.490474
moveit-demo | [ INFO] [1658754147.216886988]: Differences 0.0032894 -0.0213406 0.00952596
moveit-demo | [ INFO] [1658754147.216912792]: Orientation constraint violated for link 'tool0'. Quaternion desired: 0.000000 0.000000 0.000000 1.000000, quaternion actual: 0.001313 0.012308 0.000478 0.999923, error: x=0.002614, y=0.024618, z=0.000925, tolerance: x=0.001745, y=0.001745, z=0.001745
moveit-demo | [ INFO] [1658754147.227110026]: manipulator/manipulator[RRTConnect]: Created 1 states (1 start + 0 goal)
moveit-demo | [ INFO] [1658754147.227400497]: manipulator/manipulator[RRTConnect]: Created 1 states (1 start + 0 goal)
moveit-demo | [ INFO] [1658754147.227708370]: manipulator/manipulator[RRTConnect]: Created 1 states (1 start + 0 goal)
moveit-demo | [ INFO] [1658754147.227905721]: manipulator/manipulator[RRTConnect]: Created 1 states (1 start + 0 goal)
moveit-demo | [ INFO] [1658754147.233503222]: Unable to solve the planning problem
moveit-demo | [ INFO] [1658754461.048931581]: Received new planning service request...
moveit-demo | [ INFO] [1658754461.104096831]: Planner configuration 'manipulator[RRTConnect]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
moveit-demo | [ INFO] [1658754461.104686668]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.104957092]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.105139654]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.105365436]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.116384683]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.116958226]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
moveit-demo | [ INFO] [1658754461.126401535]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.126563813]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
moveit-demo | [ INFO] [1658754461.126730819]: ParallelPlan::solve(): Solution found by one or more threads in 0.022336 seconds
moveit-demo | [ INFO] [1658754461.127239558]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.127302273]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.127366080]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.127419972]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.127704819]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
moveit-demo | [ INFO] [1658754461.127839167]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
moveit-demo | [ INFO] [1658754461.128358171]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.129020695]: manipulator/manipulator[RRTConnect]: Created 6 states (4 start + 2 goal)[ WARN] [1658754461.315070684]: More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?
moveit-demo | [0m
moveit-demo | [ INFO] [1658754461.129271426]: ParallelPlan::solve(): Solution found by one or more threads in 0.002361 seconds
moveit-demo | [ INFO] [1658754461.130217361]: SimpleSetup: Path simplification took 0.000748 seconds and changed from 3 to 2 states
moveit-demo | [ INFO] [1658754461.148835998]: Received new planning service request...
moveit-demo | [ INFO] [1658754461.203901267]: Planner configuration 'manipulator[RRTConnect]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
moveit-demo | [ INFO] [1658754461.204651864]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.204798810]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.205050106]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.205188597]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.216600416]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.216760228]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
moveit-demo | [ INFO] [1658754461.226560796]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.226762587]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
moveit-demo | [ INFO] [1658754461.226892359]: ParallelPlan::solve(): Solution found by one or more threads in 0.022625 seconds
moveit-demo | [ INFO] [1658754461.227393304]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.227643708]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.227856305]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.228102762]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.228583312]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.228846912]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.229008022]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
moveit-demo | [ INFO] [1658754461.229332657]: manipulator/manipulator[RRTConnect]: Created 6 states (3 start + 3 goal)
moveit-demo | [ INFO] [1658754461.229517474]: ParallelPlan::solve(): Solution found by one or more threads in 0.002453 seconds
moveit-demo | [ INFO] [1658754461.232077914]: SimpleSetup: Path simplification took 0.002406 seconds and changed from 3 to 2 states
moveit-demo | [ INFO] [1658754461.252452577]: Received new planning service request...
moveit-demo | [ INFO] [1658754461.303706904]: Planner configuration 'manipulator[RRTConnect]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
moveit-demo | [ INFO] [1658754461.304288027]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.304544639]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.304863704]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.305090070]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
moveit-demo | [ INFO] [1658754461.316063343]: Constraint satisfied:: Joint name: 'shoulder_pan_joint', actual value: 4.208515, desired value: 4.175836, tolerance_above: 0.043633, tolerance_below: 0.043633
moveit-demo | [ INFO] [1658754461.316130513]: Constraint satisfied:: Joint name: 'shoulder_lift_joint', actual value: 4.445044, desired value: 4.466577, tolerance_above: 0.043633, tolerance_be[ERROR] [1658754461.325318841]: manipulator/manipulator[RRTConnect]: Unable to sample any valid states for goal tree
moveit-demo | [ERROR] [1658754461.325627649]: manipulator/manipulator[RRTConnect]: Unable to sample any valid states for goal tree
moveit-demo | [ERROR] [1658754461.325717835]: manipulator/manipulator[RRTConnect]: Unable to sample any valid states for goal tree
moveit-demo | [ERROR] [1658754461.326035956]: manipulator/manipulator[RRTConnect]: Unable to sample any valid states for goal tree
moveit-demo | [ WARN] [1658754461.326462518]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.022443 seconds
moveit-demo | [ERROR] [1658754461.326867497]: manipulator/manipulator[RRTConnect]: Insufficient states in sampleable goal region
moveit-demo | [ERROR] [1658754461.327146434]: manipulator/manipulator[RRTConnect]: Insufficient states in sampleable goal region
moveit-demo | [ERROR] [1658754461.327299193]: manipulator/manipulator[RRTConnect]: Insufficient states in sampleable goal region
moveit-demo | [ERROR] [1658754461.327685043]: manipulator/manipulator[RRTConnect]: Insufficient states in sampleable goal region
moveit-demo | [ WARN] [1658754461.327829135]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.001082 seconds
ros-bridge | 2022-07-25 13:07:41+0000 [-] [INFO] [1658754461.332589]: Client disconnected. 0 clients total.
```
**Versions**
- OS: `MacOSX 10.15.7`
- Python version `3.10`
- Python package manager `conda`
- Compas `1.16.0`
- Compas_fab `0.26.0`
- [UR10e docker file](https://gramaziokohler.github.io/compas_fab/latest/_downloads/4bd2f38e7b50405bd8d8b8af0670be1f/docker-compose.yml)
Am I doing something wrong?
Is this related to this [commit](https://github.com/compas-dev/compas_fab/commit/a6d23da9833606e61619ac5d8af42e39c1d2ffc7)?
Thanks,
Luca
Contributor guide
Assessment
This issue has not been assessed yet.