Custom limits

examples/test_tracik.py

@@ -3,16 +3,19 @@
 from __future__ import print_function
 
 import argparse
+import numpy as np
 
 from pybullet_tools.pr2_utils import PR2_URDF, DRAKE_PR2_URDF, \
     SIDE_HOLDING_LEFT_ARM, PR2_GROUPS, open_arm, REST_LEFT_ARM, rightarm_from_leftarm, PR2_TOOL_FRAMES, LEFT_ARM
 from pybullet_tools.tracik import IKSolver
 from pybullet_tools.utils import set_joint_positions, add_data_path, connect, dump_body, load_model, joints_from_names, \
     disconnect, HideOutput, load_pybullet, base_aligned_z, Point, set_point, get_aabb, \
-    FLOOR_URDF, wait_unlocked
+    FLOOR_URDF, wait_unlocked, multiply, Pose, Point
 
 
-def main():
+def main(arm=LEFT_ARM):
+    np.set_printoptions(precision=3)
+
     parser = argparse.ArgumentParser()
     parser.add_argument('--first_joint', default=None, choices=[None, 'x', 'y', 'z', 'torso_lift_joint', 'l_shoulder_pan_joint'])
     parser.add_argument('--drake', action='store_true')
@@ -32,7 +35,7 @@ def main():
     #z = stable_z_on_aabb(robot, AABB(np.zeros(3), np.zeros(3)))
     print('Base z: {:.3f}'.format(z))
     set_point(robot, Point(z=z))
-    wait_unlocked()
+    open_arm(robot, arm)
 
     group_confs = {
         'left_arm': SIDE_HOLDING_LEFT_ARM, # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
@@ -45,18 +48,22 @@ def main():
 
     ########################################
 
-    arm = LEFT_ARM
-    open_arm(robot, arm)
     tool_link = PR2_TOOL_FRAMES[LEFT_ARM]
     ik_solver = IKSolver(robot, tool_link=tool_link, first_joint=args.first_joint)
     print(ik_solver)
 
     tool_pose = ik_solver.get_tool_pose()
-    ik_solver.draw_pose(tool_pose)
-    while True:
-        #conf = ik_solver.sample_conf()
-        conf = ik_solver.solve_randomized(tool_pose)
-        print('Solution:', conf)
+    #target_pose = tool_pose
+    target_pose = multiply(Pose(point=Point(z=0.05)), tool_pose)
+    ik_solver.draw_pose(target_pose)
+    print('Target pose:', target_pose)
+    print('Lower:', ik_solver.lower_limits)
+    print('Upper:', ik_solver.upper_limits)
+    print('Initial conf:', np.array(ik_solver.get_conf()))
+    wait_unlocked('Continue')
+
+    for i, conf in enumerate(ik_solver.generate(tool_pose)):
+        print('{}) Solution: {}'.format(i, np.array(conf)))
         if conf is not None:
             ik_solver.set_conf(conf)
         wait_unlocked('Continue')

models/drake/pr2_description/urdf/pr2_simplified.urdf

@@ -94,18 +94,19 @@
   <link name="base_link_0"/>
   <link name="base_link_1"/>
   <joint name="x" type="prismatic">
-    <limit lower="-5" upper="5"/>
+    <limit lower="-5" upper="5" effort="1000.0" velocity="1.0"/>
     <parent link="world_link"/>
     <child link="base_link_0"/>
     <axis xyz="1 0 0"/>
   </joint>
   <joint name="y" type="prismatic">
-    <limit lower="-5" upper="5"/>
+    <limit lower="-5" upper="5" effort="1000.0" velocity="1.0"/>
     <parent link="base_link_0"/>
     <child link="base_link_1"/>
     <axis xyz="0 1 0"/>
   </joint>
   <joint name="theta" type="continuous">
+    <limit effort="1000.0" velocity="1.5708"/>
     <parent link="base_link_1"/>
     <child link="base_footprint"/>
     <axis xyz="0 0 1"/>

pybullet_tools/tracik.py

@@ -6,11 +6,12 @@
 
 from pybullet_tools.utils import Pose, multiply, invert, tform_from_pose, get_model_info, BASE_LINK, \
     get_link_name, link_from_name, get_joint_name, joint_from_name, parent_link_from_joint, joints_from_names, \
-    links_from_names, get_link_pose, draw_pose, set_joint_positions, get_joint_positions
+    links_from_names, get_link_pose, draw_pose, set_joint_positions, get_joint_positions, get_joint_limits, \
+    CIRCULAR_LIMITS, get_custom_limits
 
 
 class IKSolver(object):
-    def __init__(self, body, tool_link, first_joint=None, tool_offset=Pose(),
+    def __init__(self, body, tool_link, first_joint=None, tool_offset=Pose(), custom_limits={},
                  seed=None, max_time=5e-3, error=1e-5): #, **kwargs):
         self.tool_link = link_from_name(body, tool_link)
         if first_joint is None:
@@ -32,6 +33,9 @@ def __init__(self, body, tool_link, first_joint=None, tool_offset=Pose(),
             timeout=max_time, epsilon=error,
             solve_type='Speed', # Speed | Distance | Manipulation1 | Manipulation2
         )
+        self.ik_solver.joint_limits = list(get_custom_limits(
+            self.body, self.joints, custom_limits=custom_limits, circular_limits=CIRCULAR_LIMITS))
+
         self.tool_offset = tool_offset # None
         self.random_generator = np.random.RandomState(seed)
         self.solutions = []
@@ -121,7 +125,7 @@ def solve(self, tool_pose, seed_conf=None, pos_tolerance=1e-5, ori_tolerance=mat
         conf = self.ik_solver.ik(tform, qinit=seed_conf, bx=bx, by=by, bz=bz, brx=brx, bry=bry, brz=brz)
         self.solutions.append((pose, conf))
         return conf
-    def solve_closest(self, tool_pose, **kwargs):
+    def solve_current(self, tool_pose, **kwargs): # solve_closest
         return self.solve(tool_pose, seed_conf=self.get_conf(), **kwargs)
     def solve_randomized(self, tool_pose, **kwargs):
         return self.solve(tool_pose, seed_conf=self.sample_conf(), **kwargs)