0
$\begingroup$

I'm using Arduino Tinkerkit Braccio for a project and I cannot get planning to pose goal to work. I'm using KDL as IK solver, RRTConnectkConfigDefault as move_group planner_id

This is the relevant part of manipulator.srdf

<robot name="kr5">
    <group name="braccio_arm">
        <chain base_link="base_link" tip_link="gripper_grasp_link" />
    </group>
    <group name="braccio_gripper">
        <link name="right_gripper_link"/>
        <link name="left_gripper_link"/>
    </group>
    <end_effector name="braccio_gripper" parent_link="link_5" group="braccio_gripper"/>
</robot>

this is the class that i'm using with methods for planning

class BraccioPoseGoal(object):
        """BraccioPoseGoalInterface"""
        def __init__(self, node_handle):
                super(BraccioPoseGoal, self).__init__()

                self.nh = node_handle

                moveit_commander.roscpp_initialize(sys.argv)

                group_name = "braccio_arm"
                self.move_group = moveit_commander.MoveGroupCommander(group_name)
                self.move_group.set_planner_id("RRTConnectkConfigDefault")
                self.gripper_group = moveit_commander.MoveGroupCommander("braccio_gripper")

                self.random_pose = PoseStamped()

                self.display_trajectory_publisher = rospy.Publisher(
                "/move_group/display_planned_path",
                moveit_msgs.msg.DisplayTrajectory,
                queue_size=20,
                )

                ## Provides information such as the robot's
                ## kinematic model and the robot's current joint states
                self.robot = moveit_commander.RobotCommander()

                self.scene = moveit_commander.PlanningSceneInterface()



        def random(self):
                random_pose = self.move_group.get_random_pose()
                print(type(random_pose))
                print(random_pose)
                self.random_pose = random_pose
                print(self.random_pose)
                return self.random_pose

        def planning_pose_random(self):
                print(f"End effector link is {self.move_group.get_end_effector_link()}")

                pose_start = self.move_group.get_current_pose()
                print(f"pose_start is\n \
                        {pose_start}")

                self.move_group.set_planning_time(10)
                self.move_group.set_num_planning_attempts(5)
                self.move_group.set_goal_tolerance(0.5)

                print(f" pose goal is {self.random_pose}")

                self.move_group.set_pose_target(self.random_pose)

                success = self.move_group.go(wait=True)
                print(success)
                self.move_group.stop()

                self.move_group.clear_pose_targets()

        def planning_pose_example(self):
                print(f"End effector link is {self.move_group.get_end_effector_link()}")

                pose_start = self.move_group.get_current_pose()
                print(f"pose_start is\n \
                        {pose_start}")

                self.move_group.set_planning_time(10)
                self.move_group.set_num_planning_attempts(5)
                self.move_group.set_goal_tolerance(1.5)
                pose_goal = Pose()
                
                pose_goal.position.z = pose_start.pose.position.z + 0.1

                print(f" pose goal is {self.pose_goal}")

                self.move_group.set_pose_target(pose_goal)
                
                success = self.move_group.go(wait=True)
                print(success)

                self.move_group.stop()
                
                self.move_group.clear_pose_targets()

and this is the script

if __name__ == '__main__':

    rospy.init_node('planner', anonymous=True)

    targetter = pose_goal_targetter.BraccioPoseGoal(rospy)                
    
    pose_targetter.get_planning_infos()
    pose_targetter.random()
    
    pose_targetter.planning_pose_random()
    
    pose_targetter.planning_pose_example()

always get this messages

In Gazebo/Rviz simulation shell:

[ INFO] [1709295163.866945744]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1709295163.867777056]: Planning attempt 1 of at most 1
[ INFO] [1709295163.911720123]: Planner configuration 'braccio_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1709295163.939866499]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1709295163.942901919]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1709295163.946526609]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1709295163.950114213]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ERROR] [1709295173.918614429]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1709295173.919144818]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ERROR] [1709295173.920100495]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1709295173.920430642]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ERROR] [1709295173.921417567]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1709295173.922045143]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ERROR] [1709295173.927470425]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1709295173.927905711]: braccio_arm/braccio_arm[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ WARN] [1709295173.928774835]: ParallelPlan::solve(): Unable to find solution by any of the threads in 10.002176 seconds
[ WARN] [1709295173.929519232]: Timed out
[ INFO] [1709295173.951600672]: Unable to solve the planning problem

in the terminal where I launch the python script

============ Planning frame: world
============ End effector link: gripper_grasp_link
============ Available Planning Groups: ['braccio_arm', 'braccio_gripper']
============ Printing robot state
joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "world"
  name: 
    - base_joint
    - shoulder_joint
    - elbow_joint
    - wrist_pitch_joint
    - wrist_roll_joint
    - gripper_joint
    - sub_gripper_joint
  position: [0.035598356924577246, 1.7990154936831324, 2.9531272660249606, 2.468814216193401, 0.002182249418685167, 1.2566000267610828, 1.258342942562062]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "world"
  joint_names: []
  transforms: []
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False

Random pose is 
       header: 
  seq: 0
  stamp: 
    secs: 1709544802
    nsecs: 486826896
  frame_id: "world"
pose: 
  position: 
    x: -0.2041522691824122
    y: 0.12431443868630619
    z: 0.22179537180839495
  orientation: 
    x: 0.5102110813787337
    y: -0.6264556229979733
    z: -0.5554669384410957
    w: 0.1967091384543195
End effector link is gripper_grasp_link

pose_start is
             header: 
  seq: 0
  stamp: 
    secs: 1709544802
    nsecs: 588867902
  frame_id: "world"
pose: 
  position: 
    x: -0.009418271930030331
    y: -0.26445864343459313
    z: 0.06763842604372583
  orientation: 
    x: 0.6606592505786385
    y: -0.683115672339261
    z: 0.21589729718896952
    w: 0.22421126175071893
    
 Random pose goal is 
       header: 
  seq: 0
  stamp: 
    secs: 1709544802
    nsecs: 486826896
  frame_id: "world"
pose: 
  position: 
    x: -0.2041522691824122
    y: 0.12431443868630619
    z: 0.22179537180839495
  orientation: 
    x: 0.5102110813787337
    y: -0.6264556229979733
    z: -0.5554669384410957
    w: 0.1967091384543195
    

[ INFO] [1709544812.743104685]: ABORTED: TIMED_OUT
False


End effector link is gripper_grasp_link
pose_start is
             header: 
  seq: 0
  stamp: 
    secs: 1709544812
    nsecs: 787979125
  frame_id: "world"
pose: 
  position: 
    x: -0.009418271930030328
    y: -0.26445864343459313
    z: 0.06763842604372537
  orientation: 
    x: 0.6606592505786386
    y: -0.6831156723392612
    z: 0.21589729718896897
    w: 0.2242112617507183
 pose goal is position: 
  x: 0.0
  y: 0.0
  z: 0.16763842604372536
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0

[ INFO] [1709544822.925863536]: ABORTED: TIMED_OUT
False

where am I wrong?

$\endgroup$
4
  • $\begingroup$ Welcome to Robotics, caesar753. Where are you getting the error? You seem to be using print statements instead of rospy.loginfo, and I'm not seeing any plain print output in your error. Debugging your program to the point that you could identify the line or method where you're having trouble would help you narrow down the issue, but also help us narrow it down as well. Since I'm not seeing any of your print statements in the output log I'm wondering if the source of the problem is in any of the snippets you've provided or if it's happening somewhere else. $\endgroup$
    – Chuck
    Mar 1 at 16:40
  • $\begingroup$ Thanks for your answer! I posted the output only from Gazebo/Rviz simulation shell, now I have edited the post also with script shell output! $\endgroup$
    – caesar753
    Mar 4 at 9:37
  • $\begingroup$ Are you able to move your arm to a valid pose (i.e. one that you know its valid, rather than a random one)? $\endgroup$
    – Luca
    Mar 5 at 15:05
  • $\begingroup$ Thanks for the answer!If I "plan" (i.e. set values) in joint space everything works,if I use MotionPlanning in RViz (moveit_ros_visualization) setting a "random valid" pose works, how can I get a valid pose? $\endgroup$
    – caesar753
    Mar 6 at 10:31

1 Answer 1

0
$\begingroup$

Found a solution: my robot (without gripper) had 5 DOF, as I found in many posts KDL (and other IK solvers) doesn't work very well (or, better, at all) with <6DOF, I added another DOF with a fake joint and now also pose goal planning&execution works very well!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.