From fce4198abcc6528a29fbe54038afa42e92affe42 Mon Sep 17 00:00:00 2001 From: Eddie G Date: Sat, 14 Jan 2017 16:16:56 -0800 Subject: [PATCH 1/2] Need to load ikmodel Works with newest version of OpenRave. More tests now pass in Ubuntu 14.04 and up. --- python_examples/arm_to_cart_target.py | 5 +++++ python_examples/this_side_up.py | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/python_examples/arm_to_cart_target.py b/python_examples/arm_to_cart_target.py index 4f6990c1..888f8202 100644 --- a/python_examples/arm_to_cart_target.py +++ b/python_examples/arm_to_cart_target.py @@ -27,6 +27,11 @@ # BEGIN ik manip = robot.GetManipulator("rightarm") +robot.SetActiveManipulator(manip) +ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( + robot, iktype=openravepy.IkParameterization.Type.Transform6D) +if not ikmodel.load(): + ikmodel.autogenerate() init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame", filter_options = openravepy.IkFilterOptions.CheckEnvCollisions) # END ik diff --git a/python_examples/this_side_up.py b/python_examples/this_side_up.py index 6259db97..09b50047 100644 --- a/python_examples/this_side_up.py +++ b/python_examples/this_side_up.py @@ -19,6 +19,11 @@ robot = env.GetRobots()[0] manip = robot.GetManipulator("rightarm") +robot.SetActiveManipulator(manip) +ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( + robot, iktype=openravepy.IkParameterization.Type.Transform6D) +if not ikmodel.load(): + ikmodel.autogenerate() xyz_target = [.6,-.6,1] From dd8254f1d17bd000539dd79bf90456a1b666dc58 Mon Sep 17 00:00:00 2001 From: Eddie G Date: Sat, 14 Jan 2017 16:29:16 -0800 Subject: [PATCH 2/2] Updated joint values These values reflect the updated PR2 joint limits in OpenRave. --- data/arm_around_table.json | 18 +++++++++--------- python_examples/arm_to_cart_target.py | 2 +- python_examples/arm_to_joint_target.py | 2 +- python_examples/fullbody_plan.py | 6 +++--- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/data/arm_around_table.json b/data/arm_around_table.json index e3a5903d..f2ed9607 100644 --- a/data/arm_around_table.json +++ b/data/arm_around_table.json @@ -30,15 +30,15 @@ ], "init_info" : { "type" : "given_traj", - "data" : [[-1.832, -0.332, -1.011, -1.437, -1.1 , -2.106, 3.074], - [-1.622, -0.152, -0.888, -1.45 , -1.312, -1.902, 3.064], - [-1.411, 0.028, -0.764, -1.463, -1.525, -1.698, 3.055], - [-1.201, 0.208, -0.641, -1.476, -1.737, -1.493, 3.045], - [-0.99 , 0.388, -0.517, -1.489, -1.949, -1.289, 3.036], - [-0.78 , 0.567, -0.394, -1.502, -2.162, -1.085, 3.026], - [-0.569, 0.747, -0.27 , -1.515, -2.374, -0.881, 3.017], - [-0.359, 0.927, -0.147, -1.528, -2.586, -0.676, 3.007], - [-0.148, 1.107, -0.023, -1.541, -2.799, -0.472, 2.998], + "data" : [[-1.832, -0.332, -1.011, -1.437, -1.1 , -1.926, 3.074], + [-1.622, -0.152, -0.888, -1.45 , -1.312, -1.742, 3.064], + [-1.411, 0.028, -0.764, -1.463, -1.525, -1.558, 3.055], + [-1.201, 0.208, -0.641, -1.476, -1.737, -1.373, 3.045], + [-0.99 , 0.388, -0.517, -1.489, -1.949, -1.189, 3.036], + [-0.78 , 0.567, -0.394, -1.502, -2.162, -1.005, 3.026], + [-0.569, 0.747, -0.27 , -1.515, -2.374, -0.821, 3.017], + [-0.359, 0.927, -0.147, -1.528, -2.586, -0.636, 3.007], + [-0.148, 1.107, -0.023, -1.541, -2.799, -0.452, 2.998], [ 0.062, 1.287, 0.1 , -1.554, -3.011, -0.268, 2.988]] } } diff --git a/python_examples/arm_to_cart_target.py b/python_examples/arm_to_cart_target.py index 888f8202..14c6c93e 100644 --- a/python_examples/arm_to_cart_target.py +++ b/python_examples/arm_to_cart_target.py @@ -18,7 +18,7 @@ trajoptpy.SetInteractive(args.interactive) # pause every iteration, until you press 'p'. Press escape to disable further plotting robot = env.GetRobots()[0] -joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1 , -2.106, 3.074] +joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1 , -1.926, 3.074] robot.SetDOFValues(joint_start, robot.GetManipulator('rightarm').GetArmIndices()) quat_target = [1,0,0,0] # wxyz diff --git a/python_examples/arm_to_joint_target.py b/python_examples/arm_to_joint_target.py index c53c5401..55dcf7fc 100644 --- a/python_examples/arm_to_joint_target.py +++ b/python_examples/arm_to_joint_target.py @@ -16,7 +16,7 @@ trajoptpy.SetInteractive(args.interactive) # pause every iteration, until you press 'p'. Press escape to disable further plotting robot = env.GetRobots()[0] -joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1 , -2.106, 3.074] +joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1 , -1.926, 3.074] robot.SetDOFValues(joint_start, robot.GetManipulator('rightarm').GetArmIndices()) joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988] diff --git a/python_examples/fullbody_plan.py b/python_examples/fullbody_plan.py index 926404d2..e4dabc93 100644 --- a/python_examples/fullbody_plan.py +++ b/python_examples/fullbody_plan.py @@ -62,7 +62,7 @@ def main(): # Set up robot in initial state robot.SetDOFValues([ 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, - 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, + 0.000e+00, 0.000e+00, 0.0115e+00, 0.000e+00, 0.000e+00, 6.648e-01, -3.526e-01, 1.676e+00, -1.924e+00, 2.921e+00, -1.217e+00, 1.343e+00, 0.000e+00, -4.163e-16, 0.000e+00, -1.665e-16, 0.000e+00, -6.365e-01, 9.806e-02, -1.226e+00, @@ -75,10 +75,10 @@ def main(): DOF = openravepy.DOFAffine robot.SetActiveDOFs(np.r_[robot.GetJoint("torso_lift_joint").GetDOFIndex(), robot.GetManipulator("leftarm").GetArmIndices(), robot.GetManipulator("rightarm").GetArmIndices()], DOF.X | DOF.Y | DOF.RotationAxis) - robot.SetActiveDOFValues([ 0. , 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166, + robot.SetActiveDOFValues([ 0.115 , 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166, 1.3425, -0.6365, 0.0981, -1.226 , -2.0264, -3.0125, -1.3958, -1.9289, 2.9295, 0.5748, -3.137 ]) - target_joints = [ 0. , 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005, + target_joints = [ 0.115 , 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005, 1.5994, -0.6929, -0.3338, -1.292 , -1.9048, -2.6915, -1.2908, -1.7152, 1.3155, 0.6877, -0.0041]