22from __future__ import division
33from __future__ import print_function
44
5+ import math
6+ import sys
7+
8+ from compas .robots import Joint
59from compas_fab .backends .interfaces import InverseKinematics
10+ from compas_fab .backends .pybullet .conversions import pose_from_frame
11+ from compas_fab .backends .pybullet .exceptions import InverseKinematicsError
12+ from compas_fab .robots import Configuration
13+ from compas_fab .utilities import LazyLoader
14+
15+ pybullet = LazyLoader ('pybullet' , globals (), 'pybullet' )
16+
617
718__all__ = [
819 'PyBulletInverseKinematics' ,
@@ -15,46 +26,107 @@ def __init__(self, client):
1526 self .client = client
1627
1728 def inverse_kinematics (self , frame_WCF , start_configuration = None , group = None , options = None ):
18- # """Calculate the robot's inverse kinematic for a given frame.
19- #
20- # Parameters
21- # ----------
22- # frame_WCF: :class:`compas.geometry.Frame`
23- # The frame to calculate the inverse for.
24- # start_configuration: :class:`compas_fab.robots.Configuration`, optional
25- # If passed, the inverse will be calculated such that the calculated
26- # joint positions differ the least from the start_configuration.
27- # Defaults to the init configuration.
28- # group: str, optional
29- # The planning group used for calculation. Defaults to the robot's
30- # main planning group.
31- # options: dict, optional
32- # Dictionary containing the following key-value pairs:
33- #
34- # - ``"base_link"``: (:obj:`str`) Name of the base link.
35- # - ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions.
36- # Defaults to `True`.
37- # - ``"constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional)
38- # A set of constraints that the request must obey. Defaults to `None`.
39- # - ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts.
40- # Defaults to `8`.
41- # - ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional)
42- # Defaults to `None`.
43- #
44- # Returns
45- # -------
46- # :class:`compas_fab.robots.Configuration`
47- # The planning group's configuration.
48- # """
49- raise NotImplementedError
50- # """
51- # """
52- # options = options or {}
53- # self.ensure_robot()
54- # ee_link_name = options.get('ee_link_name')
55- # pb_ee_link = self.client.link_from_name(self.client.robot_uid, ee_link_name)
56- # joint_names = [name.decode('UTF-8') for name in self.client.get_joint_names(self.robot_uid, self.client.get_movable_joints(self.client.robot_uid))]
57- # joint_positions = inverse_kinematics(self.client.robot_uid, pb_ee_link, self.client.pose_from_frame(frame), max_iterations=attempts)
58- # if not joint_positions:
59- # raise InverseKinematicsError()
60- # return joint_positions, joint_names
29+ """Calculate the robot's inverse kinematic for a given frame.
30+
31+ Parameters
32+ ----------
33+ frame_WCF: :class:`compas.geometry.Frame`
34+ The frame to calculate the inverse for.
35+ start_configuration: :class:`compas_fab.robots.Configuration`, optional
36+ If passed, the inverse will be calculated such that the calculated
37+ joint positions differ the least from the start_configuration.
38+ Defaults to the init configuration.
39+ group: str, optional
40+ The planning group used for calculation. Defaults to the robot's
41+ main planning group.
42+ options: dict, optional
43+ Dictionary containing the following key-value pairs:
44+
45+ -``"robot"``: (:class:`compas_fab.robots.Robot) Robot for which to compute
46+ the inverse kinematics.
47+ - ``"link_name"``: (:obj:`str`, optional ) Name of the link for which
48+ to compute the inverse kinematics. Defaults to the given robot's end
49+ effector.
50+ - ``"semi-constrained"``: (:obj:`bool`, optional) When ``True``, only the
51+ position and not the orientation of ``frame_WCF`` will not be considered
52+ in the calculation. Defaults to ``False``.
53+ - ``"enforce_joint_limits"``: (:obj:`bool`, optional) When ``False``, the
54+ robot's joint limits will be ignored in the calculation. Defaults to
55+ ``True``.
56+
57+ Returns
58+ -------
59+ :class:`compas_fab.robots.Configuration`
60+ The planning group's configuration.
61+
62+ Raises
63+ ------
64+ :class:`compas_fab.backends.pybullet.InverseKinematicsError`
65+ """
66+ robot = options ['robot' ]
67+ link_name = options .get ('link_name' ) or robot .get_end_effector_link_name (group )
68+ link_id = self .client ._get_link_id_by_name (link_name , robot )
69+ point , orientation = pose_from_frame (frame_WCF )
70+
71+ joints = robot .get_configurable_joints ()
72+ joints .sort (key = lambda j : j .attr ['pybullet' ]['id' ])
73+ joint_names = [joint .name for joint in joints ]
74+ joint_types = [joint .type for joint in joints ]
75+
76+ if start_configuration :
77+ start_configuration = self .client .set_robot_configuration (robot , start_configuration , group )
78+
79+ called_from_test = 'pytest' in sys .modules
80+ if options .get ('enforce_joint_limits' , True ) and not called_from_test :
81+
82+ lower_limits = [joint .limit .lower if joint .type != Joint .CONTINUOUS else 0 for joint in joints ]
83+ upper_limits = [joint .limit .upper if joint .type != Joint .CONTINUOUS else 2 * math .pi for joint in joints ]
84+ # I don't know what jointRanges needs to be. Erwin Coumans knows, but he isn't telling.
85+ # https://stackoverflow.com/questions/49674179/understanding-inverse-kinematics-pybullet
86+ # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/preview?pru=AAABc7276PI*zazLer2rlZ8tAUI8lF98Kw#heading=h.9i02ojf4k3ve
87+ joint_ranges = [u - l for u , l in zip (upper_limits , lower_limits )]
88+ rest_configuration = start_configuration or robot .zero_configuration ()
89+ rest_poses = self ._get_rest_poses (joint_names , rest_configuration )
90+
91+ if options .get ('semi-constrained' ):
92+ joint_positions = pybullet .calculateInverseKinematics (
93+ robot .pybullet_uid ,
94+ link_id ,
95+ point ,
96+ lowerLimits = lower_limits ,
97+ upperLimits = upper_limits ,
98+ jointRanges = joint_ranges ,
99+ restPoses = rest_poses ,
100+ )
101+ else :
102+ joint_positions = pybullet .calculateInverseKinematics (
103+ robot .pybullet_uid ,
104+ link_id ,
105+ point ,
106+ orientation ,
107+ lowerLimits = lower_limits ,
108+ upperLimits = upper_limits ,
109+ jointRanges = joint_ranges ,
110+ restPoses = rest_poses ,
111+ )
112+ else :
113+ if options .get ('semi-constrained' ):
114+ joint_positions = pybullet .calculateInverseKinematics (
115+ robot .pybullet_uid ,
116+ link_id ,
117+ point ,
118+ )
119+ else :
120+ joint_positions = pybullet .calculateInverseKinematics (
121+ robot .pybullet_uid ,
122+ link_id ,
123+ point ,
124+ orientation ,
125+ )
126+ if not joint_positions :
127+ raise InverseKinematicsError ()
128+ return Configuration (joint_positions , joint_types , joint_names )
129+
130+ def _get_rest_poses (self , joint_names , configuration ):
131+ name_value_map = {configuration .joint_names [i ]: configuration .values [i ] for i in range (len (configuration .joint_names ))}
132+ return [name_value_map [name ] for name in joint_names ]
0 commit comments