Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

IK Solution differ from motion planning solution? #364

Open
imwhocodes opened this issue Jul 25, 2022 · 1 comment
Open

IK Solution differ from motion planning solution? #364

imwhocodes opened this issue Jul 25, 2022 · 1 comment

Comments

@imwhocodes
Copy link

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

  • 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

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

Thanks,
Luca
@gonzalocasas
Copy link
Member

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!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@gonzalocasas @imwhocodes