trac_ik.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Author: Sammy Pfeiffer <Sammy.Pfeiffer at student.uts.edu.au>
4 # Convenience code to wrap TRAC IK
5 
6 from trac_ik_python.trac_ik_wrap import TRAC_IK
7 import rospy
8 
9 
10 class IK(object):
11  def __init__(self, base_link, tip_link,
12  timeout=0.005, epsilon=1e-5, solve_type="Speed",
13  urdf_string=None):
14  """
15  Create a TRAC_IK instance and keep track of it.
16 
17  :param str base_link: Starting link of the chain.
18  :param str tip_link: Last link of the chain.
19  :param float timeout: Timeout in seconds for the IK calls.
20  :param float epsilon: Error epsilon.
21  :param solve_type str: Type of solver, can be:
22  Speed (default), Distance, Manipulation1, Manipulation2
23  :param urdf_string str: Optional arg, if not given URDF is taken from
24  the param server at /robot_description.
25  """
26  if urdf_string is None:
27  urdf_string = rospy.get_param('/robot_description')
28  self._urdf_string = urdf_string
29  self._timeout = timeout
30  self._epsilon = epsilon
31  self._solve_type = solve_type
32  self.base_link = base_link
33  self.tip_link = tip_link
35  self.tip_link,
36  self._urdf_string,
37  self._timeout,
38  self._epsilon,
39  self._solve_type)
40  self.number_of_joints = self._ik_solver.getNrOfJointsInChain()
41  self.joint_names = self._ik_solver.getJointNamesInChain(
42  self._urdf_string)
43  self.link_names = self._ik_solver.getLinkNamesInChain()
44 
45  def get_ik(self, qinit,
46  x, y, z,
47  rx, ry, rz, rw,
48  bx=1e-5, by=1e-5, bz=1e-5,
49  brx=1e-3, bry=1e-3, brz=1e-3):
50  """
51  Do the IK call.
52 
53  :param list of float qinit: Initial status of the joints as seed.
54  :param float x: X coordinates in base_frame.
55  :param float y: Y coordinates in base_frame.
56  :param float z: Z coordinates in base_frame.
57  :param float rx: X quaternion coordinate.
58  :param float ry: Y quaternion coordinate.
59  :param float rz: Z quaternion coordinate.
60  :param float rw: W quaternion coordinate.
61  :param float bx: X allowed bound.
62  :param float by: Y allowed bound.
63  :param float bz: Z allowed bound.
64  :param float brx: rotation over X allowed bound.
65  :param float bry: rotation over Y allowed bound.
66  :param float brz: rotation over Z allowed bound.
67 
68  :return: joint values or None if no solution found.
69  :rtype: tuple of float.
70  """
71  if len(qinit) != self.number_of_joints:
72  raise Exception("qinit has length %i and it should have length %i" % (
73  len(qinit), self.number_of_joints))
74  solution = self._ik_solver.CartToJnt(qinit,
75  x, y, z,
76  rx, ry, rz, rw,
77  bx, by, bz,
78  brx, bry, brz)
79  if solution:
80  return solution
81  else:
82  return None
83 
84  def get_joint_limits(self):
85  """
86  Return lower bound limits and upper bound limits for all the joints
87  in the order of the joint names.
88  """
89  lb = self._ik_solver.getLowerBoundLimits()
90  ub = self._ik_solver.getUpperBoundLimits()
91  return lb, ub
92 
93  def set_joint_limits(self, lower_bounds, upper_bounds):
94  """
95  Set joint limits for all the joints.
96 
97  :arg list lower_bounds: List of float of the lower bound limits for
98  all joints.
99  :arg list upper_bounds: List of float of the upper bound limits for
100  all joints.
101  """
102  if len(lower_bounds) != self.number_of_joints:
103  raise Exception("lower_bounds array size mismatch, it's size %i, should be %i" % (
104  len(lower_bounds),
105  self.number_of_joints))
106 
107  if len(upper_bounds) != self.number_of_joints:
108  raise Exception("upper_bounds array size mismatch, it's size %i, should be %i" % (
109  len(upper_bounds),
110  self.number_of_joints))
111  self._ik_solver.setKDLLimits(lower_bounds, upper_bounds)
def get_ik(self, qinit, x, y, z, rx, ry, rz, rw, bx=1e-5, by=1e-5, bz=1e-5, brx=1e-3, bry=1e-3, brz=1e-3)
Definition: trac_ik.py:49
def set_joint_limits(self, lower_bounds, upper_bounds)
Definition: trac_ik.py:93
def __init__(self, base_link, tip_link, timeout=0.005, epsilon=1e-5, solve_type="Speed", urdf_string=None)
Definition: trac_ik.py:13
def get_joint_limits(self)
Definition: trac_ik.py:84


trac_ik_python
Author(s): Sam Pfeiffer
autogenerated on Tue Apr 23 2019 02:39:19