search

compas-dev / compas_fab

Robotic fabrication package for the COMPAS Framework.
https://compas.dev/compas_fab/
MIT License
108 stars 32 forks source link

IK Solution differ from motion planning solution? #364

Open imwhocodes opened 1 year ago

imwhocodes commented 1 year ago

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 <module>
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

Am I doing something wrong? Is this related to this commit?

Thanks, Luca

gonzalocasas commented 1 year ago

Hi Luca,

Thanks for the super detailed issue! First of all, this is not related to the commit you mention, in this case you're using MoveIt! planner, and the commit only changes the in-memory python IK solver.

I tried changing planners and tweaking settings, but it keeps failing. Some planners output a bit more info that seems to point to the fact that the problem is simply too constrained to solve (despite the fact that the first two constraints are effectively redundant), but if you see this output, it really complains that the joint constraints can be satisfied but the other ones (created by the frame) are violated:

[ INFO] [1660986856.598311300]: Planner configuration 'manipulator[RRT]' will use planner 'geometric::RRT'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1660986856.608948600]: Constraint satisfied:: Joint name: 'shoulder_pan_joint', actual value: 0.577611, desired value: 0.536553, tolerance_above: 0.043633, tolerance_below: 0.043633
[ INFO] [1660986856.609133200]: Constraint satisfied:: Joint name: 'shoulder_lift_joint', actual value: -1.019137, desired value: -1.005746, tolerance_above: 0.043633, tolerance_below: 0.043633
[ INFO] [1660986856.609173400]: Constraint satisfied:: Joint name: 'elbow_joint', actual value: 1.595917, desired value: 1.588866, tolerance_above: 0.043633, tolerance_below: 0.043633
[ INFO] [1660986856.609214600]: Constraint satisfied:: Joint name: 'wrist_1_joint', actual value: -5.254482, desired value: -5.295509, tolerance_above: 0.043633, tolerance_below: 0.043633
[ INFO] [1660986856.609256800]: Constraint satisfied:: Joint name: 'wrist_2_joint', actual value: 4.736449, desired value: 4.712389, tolerance_above: 0.043633, tolerance_below: 0.043633
[ INFO] [1660986856.609283700]: Constraint satisfied:: Joint name: 'wrist_3_joint', actual value: 1.027779, desired value: 1.034244, tolerance_above: 0.043633, tolerance_below: 0.043633
[ INFO] [1660986856.609362000]: Position constraint violated on link 'tool0'. Desired: 0.500000, 0.500000, 0.500000, current: 0.476804, 0.521981, 0.511430
[ INFO] [1660986856.609438900]: Differences 0.0231964 -0.0219812 -0.0114302
[ INFO] [1660986856.609482600]: Orientation constraint violated for link 'tool0'. Quaternion desired: 0.000000 0.000000 0.000000 1.000000, quaternion actual: -0.019405 0.008297 0.017501 0.999624, error: x=0.039100, y=0.015910, z=0.035322, tolerance: x=0.001745, y=0.001745, z=0.001745
[ INFO] [1660986856.633684300]: manipulator/manipulator[RRT]: Starting planning with 1 states already in datastructure
[ INFO] [1660986856.635383600]: manipulator/manipulator[RRT]: Starting planning with 1 states already in datastructure
[ INFO] [1660986856.635997300]: manipulator/manipulator[RRT]: Starting planning with 1 states already in datastructure
[ INFO] [1660986856.637133200]: manipulator/manipulator[RRT]: Starting planning with 1 states already in datastructure
[ WARN] [1660986886.599700400]: ParallelPlan::solve(): Unable to find solution by any of the threads in 29.975050 seconds
[ WARN] [1660986903.907469200]: More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?

Why would be the reason to define these goal with an redundant constraint, is it only to test?

Cheers!