search

tork-a / rtmros_nextage

ROS-OpenRTM-based opensource robot controller software for dual-armed robot Nextage from Kawada Industries
http://wiki.ros.org/rtmros_nextage
27 stars 39 forks source link

Issue planning dual-arm motions (with torso) from Python #214

Open DamienSalle opened 8 years ago

DamienSalle commented 8 years ago

Hi.

I'm testing the Hydro version of the new Moveit configuration that allows dual-arm motion planning. Got sources from Github.

Everything if fine when using the Rviz Motion planning interface: when allowing to "Replan", any random configuration can be planned and executed.

The issue is when trying to plan the same motion from Python.

In the attached python test code (attention, indentation is not ok in the snippet), I got the different Target Pose from various configurations that MoveIt was able to reach when specified in Rviz (executed the motion and afterwards appointed the current pose). When running the python code, and testing various planner configurations, I always get an error from Moveit:

[ERROR] [1452008585.865444229, 465.114999999]: botharms[LBKPIECEkConfigDefault]: Unable to sample any valid states for goal tree

When the goal doesn't need any torso rotation to be reached, runs fine. But everytime we need a torso rotation, the planner is failing...

Any idea where the issue can be?

Thanks Damien

#!/usr/bin/env python
##########################################

import moveit_commander
import rospy
import tf

from moveit_commander import RobotCommander, PlanningSceneInterface

import roslib
import math

from geometry_msgs.msg import Pose

def main():
        rospy.init_node("flexbotics_demos_bimanual_tests")

    robot = RobotCommander()
    print "Available groups: ",robot.get_group_names()

        botharms = moveit_commander.MoveGroupCommander("botharms")

    # Allow replanning to increase the odds of a solution
    botharms.allow_replanning(True)
    botharms.set_start_state_to_current_state()
    botharms.set_planner_id("LBKPIECEkConfigDefault") 
    botharms.set_planning_time(5.0)

        # Give the scene a chance to catch up
        rospy.sleep(2)

##Define Pose1
        target_pose_r1 = Pose()
        target_pose_l1 = Pose()
        q = tf.transformations.quaternion_from_euler(-1.96522604141, -1.57025098009,1.96453622421)
        target_pose_r1.position.x = 0.23
        target_pose_r1.position.y = -0.2
        target_pose_r1.position.z = 0.2
        target_pose_r1.orientation.x = q[0]
        target_pose_r1.orientation.y = q[1]
        target_pose_r1.orientation.z = q[2]
        target_pose_r1.orientation.w = q[3]

        target_pose_l1.position.x = 0.23
        target_pose_l1.position.y = 0.2
        target_pose_l1.position.z = 0.2
        target_pose_l1.orientation.x = q[0]
        target_pose_l1.orientation.y = q[1]
        target_pose_l1.orientation.z = q[2]
        target_pose_l1.orientation.w = q[3]

##Define Pose2
        target_pose_r2 = Pose()
        target_pose_l2 = Pose()
        target_pose_r2.position.x = 0.3
        target_pose_r2.position.y =-0.2
        target_pose_r2.position.z = 0.0
    target_pose_r2.orientation = target_pose_r1.orientation
        target_pose_l2.position.x = 0.3
        target_pose_l2.position.y = 0.2
        target_pose_l2.position.z = 0.0
    target_pose_l2.orientation = target_pose_l1.orientation

##Define Pose3
        target_pose_r3 = Pose()
        target_pose_l3 = Pose()
        q_r3 = tf.transformations.quaternion_from_euler(0.957735301005, 0.975219938396,0.0877124416644)
        target_pose_r3.position.x = 0.247557163762
        target_pose_r3.position.y = 0.395946109814 
        target_pose_r3.position.z = 0.417518938127
        target_pose_r3.orientation.x = q_r3[0]
        target_pose_r3.orientation.y = q_r3[1]
        target_pose_r3.orientation.z = q_r3[2]
        target_pose_r3.orientation.w = q_r3[3]

        q_l3 = tf.transformations.quaternion_from_euler(2.16471454577, -0.091631345067,-1.52963098983)
        target_pose_l3.position.x = -0.2212569832
        target_pose_l3.position.y = 0.316437698382
        target_pose_l3.position.z = 0.293698810605
        target_pose_l3.orientation.x = q_l3[0]
        target_pose_l3.orientation.y = q_l3[1]
        target_pose_l3.orientation.z = q_l3[2]
        target_pose_l3.orientation.w = q_l3[3]

##Move to Pose1
        botharms.set_pose_target(target_pose_r1, 'RARM_JOINT5_Link')
        botharms.set_pose_target(target_pose_l1, 'LARM_JOINT5_Link')
        botharms.go()
        rospy.sleep(3)

##Move to Pose2
        botharms.set_pose_target(target_pose_r2, 'RARM_JOINT5_Link')
        botharms.set_pose_target(target_pose_l2, 'LARM_JOINT5_Link')
        botharms.go()
        rospy.sleep(3)

##Move to Pose3
        botharms.set_pose_target(target_pose_r3, 'RARM_JOINT5_Link')
        botharms.set_pose_target(target_pose_l3, 'LARM_JOINT5_Link')
        botharms.go()
        rospy.sleep(3)

##Move to Pose1
        botharms.set_pose_target(target_pose_r1, 'RARM_JOINT5_Link')
        botharms.set_pose_target(target_pose_l1, 'LARM_JOINT5_Link')
        botharms.go()
        rospy.sleep(3)

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
130s commented 8 years ago

I did run your script and it seems to me I'm running into the same issue. Need to look into more.

(Meanwhile, do you have specific reason to use LBKPIECEkConfigDefault? I know in https://github.com/tork-a/rtmros_nextage/issues/170#issuecomment-107355938 LBK might be working fine for you but we're not yet sure, so at least I keep using RRT*.)

DamienSalle commented 8 years ago

Hi. No, no specific reason. I tested various planners. In Rviz Moveit panel, LBKPIECE1 is used by default and the planner does provide a solution. So i wanted to change as little variables as possible...

Not sure if it helps you find a solution, but this post might be useful... https://groups.google.com/d/topic/moveit-users/vV5GR2kFR9o/discussion

130s commented 8 years ago

@TecnaliaRobotics

Everything if fine when using the Rviz Motion planning interface: when allowing to "Replan", any random configuration can be planned and executed.

Have you confirmed that the path (start pose and goal pose) you're giving in your python script works on RViz? If so screenshot/videoshot of it might be helpful, as well as the reproduction steps on RViz.

DamienSalle commented 8 years ago

Yes I do confirm this. The steps where

  1. Find in rviz a goal config that would request torso rotation to be reached.
  2. Check in rviz/moveit that the planner finds a solution (always allow 'múltiple attemps ' otherwise never finds a decent plan')
  3. Apply motion in rviz (simulation)
  4. Write down ‎final robot pose
  5. Go back to initianl position
  6. Ask in the python script‎ to go to the configuration found in 2 and 4.

The python script fails everytime you need a torso rotation.

I don't know how to specify in rviz a given pose as target pose... so can't replicate the motion you found.

I made a video of the final result I think. I'll send it when I'm at the office.

Damien

Enviado desde mi smartphone BlackBerry 10. De: Isaac I.Y. Saito Enviado: jueves, 28 de enero de 2016 07:25 Para: tork-a/rtmros_nextage Responder a: tork-a/rtmros_nextage CC: Salle, Damien Asunto: Re: [rtmros_nextage] Issue planning dual-arm motions (with torso) from Python (#214)

@TecnaliaRoboticshttps://github.com/TecnaliaRobotics

Everything if fine when using the Rviz Motion planning interface: when allowing to "Replan", any random configuration can be planned and executed.

Have you confirmed that the path (start pose and goal pose) you're giving in your python script works on RViz? If so screenshot/videoshot of it might be helpful, as well as the reproduction steps on RViz.

— Reply to this email directly or view it on GitHubhttps://github.com/tork-a/rtmros_nextage/issues/214#issuecomment-176009214.

DamienSalle commented 8 years ago

Find attached a video I generated when testing the full-body moveit planning in rviz, with a combined motion of the torso + arms + head...

Tecnalia_MoveIT_NextageWholeBody.zip

130s commented 8 years ago

Thanks for the video.

So far our guess is that what you want to do, ie. to move both arms and torso by specifying EEF poses using inverse kinematics, might not be possible.

Although I haven't looked into, I doubt MoveIt! RViz plugin only passes EEF poses to MoveIt! internal processes. Instead I assume plugin computes the joint angles of the given goal pose and pass those values to MoveIt! (there shouldn't be any hurdle for doing this way particular to a move groups that contains both arms and torso). If someone can confirm this that would be great.

I could be wrong (I hope so!).

Another thought is the RViz plugin might be using C++ interface of MoveGroup, so if you write the behavior in C++ things may be different? (but this is less likely, since python MoveGroupCommander is simply calling its C++ equivalent).

130s commented 8 years ago

Although I haven't had a gear to try this, I'm curious if using IKFast makes any different. We have a package dedicated for IKFast config.

k-okada commented 8 years ago

it won't help in this situation.

◉ Kei Okada

On Sat, Feb 6, 2016 at 3:58 AM, Isaac I.Y. Saito notifications@github.com wrote:

Although I haven't had a gear to try this, I'm curious if using IKFast makes any different. We have a package dedicated for IKFast config https://github.com/tork-a/rtmros_nextage/tree/indigo-devel/nextage_ik_plugin .

— Reply to this email directly or view it on GitHub https://github.com/tork-a/rtmros_nextage/issues/214#issuecomment-180499497 .

DamienSalle commented 8 years ago

Hi all. This is still a blocking issue for Tecnalia...

Could you make any advances in solving the whole-torso control? I mean, from two hand cartesian poses, compute the complete path including computing the torso rotation? Thanks

k-okada commented 8 years ago

As far as I understand, we need to update kdl plugin of moveit. If you know any other moveit package that is able to control both arm + torso, please let me know.

however, if you specify two hand cartesian poses + torso rotation angle , then moeve it can compute trajectory including torso angle, and usually target torso rotation angle could be the direction to the center of two hands.

◉ Kei Okada

On Mon, Apr 4, 2016 at 9:47 PM, TecnaliaRobotics notifications@github.com wrote:

Hi all. This is still a blocking issue for Tecnalia...

Could you make any advances in solving the whole-torso control? I mean, from two hand cartesian poses, compute the complete path including computing the torso rotation? Thanks

— You are receiving this because you commented. Reply to this email directly or view it on GitHub https://github.com/tork-a/rtmros_nextage/issues/214#issuecomment-205282672

130s commented 8 years ago

May or may not be related issue with Baxter https://github.com/ros-planning/moveit_robots/pull/42

130s commented 8 years ago

@TecnaliaRobotics

DamienSalle commented 8 years ago

I didn’t try with this update… Did you run the script I sent you to check if this updates solves it?

De: Isaac I.Y. Saito [mailto:notifications@github.com] Enviado el: martes, 18 de octubre de 2016 11:11 Para: tork-a/rtmros_nextage CC: Salle, Damien; Mention Asunto: Re: [tork-a/rtmros_nextage] Issue planning dual-arm motions (with torso) from Python (#214)

@TecnaliaRoboticshttps://github.com/TecnaliaRobotics

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/tork-a/rtmros_nextage/issues/214#issuecomment-254450910, or mute the threadhttps://github.com/notifications/unsubscribe-auth/AIVSggSDisAtVnpMqlanmqG7Ya_2aIBHks5q1I0OgaJpZM4G-7Iv.