Merge pull request #18 from joschu/ubuntu_fix

Ubuntu fix
joschu · May 23, 2017 · 9c634c0 · 9c634c0
2 parents 05550a8 + dd8254f
commit 9c634c0
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 14 deletions.
diff --git 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
@@ -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
@@ -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/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
@@ -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]                            
 

diff --git 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]