Skip to content

Commit

Permalink
[Python] Extract common procedure for get* geometry methods.
Browse files Browse the repository at this point in the history 
[Python] Delegate raising RuntimeError to a common method.
  • Loading branch information
@130s
130s committed Mar 16, 2017
1 parent 0acea2b commit fc022e8
Showing 1 changed file with 67 additions and 33 deletions.
100 changes: 67 additions & 33 deletions python/hrpsys_config.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1206,6 +1206,43 @@ def getJointAngles(self):
'''
return [x * 180.0 / math.pi for x in self.sh_svc.getCommand().jointRefs]

def _get_geometry(self, method, frame_name=None):
'''!@brief
A method only inteded for class-internal usage.
@since: 315.12.1 or higher
@type method: A Python function object.
@param method: One of the following Python function objects defined in class HrpsysConfigurator:
[getCurrentPose, getCurrentPosition, getCurrentReference, getCurrentRPY,
getReferencePose, getReferencePosition, getReferenceReference, getReferenceRPY]
@param frame_name str: set reference frame name (available from version 315.2.5)
@rtype: {str: [float]}
@return: Dictionary of the values for each kinematic group.
Example (using getCurrentPosition):
[{CHEST_JOINT0: [0.0, 0.0, 0.0]},
{HEAD_JOINT1: [0.0, 0.0, 0.5694999999999999]},
{RARM_JOINT5: [0.3255751238415409, -0.18236314012331808, 0.06762452267747099]},
{LARM_JOINT5: [0.3255751238415409, 0.18236314012331808, 0.06762452267747099]}]
'''
_geometry_methods = ['getCurrentPose', 'getCurrentPosition',
'getCurrentReference', 'getCurrentRPY',
'getReferencePose', 'getReferencePosition',
'getReferenceReference', 'getReferenceRPY']
if method.__name__ not in _geometry_methods:
raise RuntimeError("Passed method {} is not supported.".format(method))
for kinematic_group in self.Groups:
# The last element is usually an eef in each kinematic group,
# although not required so.
eef_name = kinematic_group[1][-1]
try:
result = method(eef_name, frame_name)
except RuntimeError as e:
print(str(e))
print("{}: {}".format(eef_name, method(eef_name)))
raise RuntimeError("Since no link name passed, ran it for all"
" available eefs.")

def getCurrentPose(self, lname=None, frame_name=None):
'''!@brief
Returns the current physical pose of the specified joint in the joint space.
Expand Down Expand Up @@ -1246,10 +1283,7 @@ def getCurrentPose(self, lname=None, frame_name=None):
\endverbatim
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getCurrentPose(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getCurrentPosition, frame_name)
if frame_name:
lname = lname + ':' + frame_name
if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname:
Expand All @@ -1270,17 +1304,17 @@ def getCurrentPosition(self, lname=None, frame_name=None):
[0.325, 0.182, 0.074]
\endverbatim
For lname, when either None passed / no lname is specified, all
eefs will be iterated and *only print* the result (i.e. nothing
returned). See _get_geometry for more info.
@type lname: str
@param lname: Name of the link.
@param lname: Name of the link.
@param frame_name str: set reference frame name (from 315.2.5)
@rtype: list of float
@return: List of x, y, z positions about the specified joint.
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getCurrentPosition(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getCurrentPosition, frame_name)
pose = self.getCurrentPose(lname, frame_name)
return [pose[3], pose[7], pose[11]]

Expand All @@ -1289,6 +1323,9 @@ def getCurrentRotation(self, lname=None, frame_name=None):
Returns the current physical rotation of the specified joint.
cf. getReferenceRotation that returns commanded value.
For lname, when either None passed / no lname is specified, all
eefs will be iterated and *only print* the result (i.e. nothing
returned). See _get_geometry for more info.
@type lname: str
@param lname: Name of the link.
@param frame_name str: set reference frame name (from 315.2.5)
Expand All @@ -1302,10 +1339,7 @@ def getCurrentRotation(self, lname=None, frame_name=None):
\endverbatim
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getCurrentRotation(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getCurrentRotation, frame_name)
pose = self.getCurrentPose(lname, frame_name)
return [pose[0:3], pose[4:7], pose[8:11]]

Expand All @@ -1314,24 +1348,27 @@ def getCurrentRPY(self, lname=None, frame_name=None):
Returns the current physical rotation in RPY of the specified joint.
cf. getReferenceRPY that returns commanded value.
For lname, when either None passed / no lname is specified, all
eefs will be iterated and *only print* the result (i.e. nothing
returned). See _get_geometry for more info.
@type lname: str
@param lname: Name of the link.
@param frame_name str: set reference frame name (from 315.2.5)
@rtype: list of float
@return: List of orientation in rpy form about the specified joint.
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getCurrentRPY(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getCurrentRPY, frame_name)
return euler_from_matrix(self.getCurrentRotation(lname), 'sxyz')

def getReferencePose(self, lname=None, frame_name=None):
'''!@brief
Returns the current commanded pose of the specified joint in the joint space.
cf. getCurrentPose that returns physical pose.
For lname, when either None passed / no lname is specified, all
eefs will be iterated and *only print* the result (i.e. nothing
returned). See _get_geometry for more info.
@type lname: str
@param lname: Name of the link.
@param frame_name str: set reference frame name (from 315.2.5)
Expand All @@ -1346,10 +1383,7 @@ def getReferencePose(self, lname=None, frame_name=None):
\endverbatim
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getReferencePose(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getReferencePose, frame_name)
if frame_name:
lname = lname + ':' + frame_name
if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname:
Expand All @@ -1364,6 +1398,9 @@ def getReferencePosition(self, lname=None, frame_name=None):
Returns the current commanded position of the specified joint in Cartesian space.
cf. getCurrentPosition that returns physical value.
For lname, when either None passed / no lname is specified, all
eefs will be iterated and *only print* the result (i.e. nothing
returned). See _get_geometry for more info.
@type lname: str
@param lname: Name of the link.
@param frame_name str: set reference frame name (from 315.2.5)
Expand All @@ -1372,10 +1409,7 @@ def getReferencePosition(self, lname=None, frame_name=None):
in the member variable 'Groups' (eg. chest, head1, head2, ..).
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getReferencePosition(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getReferencePosition, frame_name)
pose = self.getReferencePose(lname, frame_name)
return [pose[3], pose[7], pose[11]]

Expand All @@ -1384,6 +1418,9 @@ def getReferenceRotation(self, lname=None, frame_name=None):
Returns the current commanded rotation of the specified joint.
cf. getCurrentRotation that returns physical value.
For lname, when either None passed / no lname is specified, all
eefs will be iterated and *only print* the result (i.e. nothing
returned). See _get_geometry for more info.
@type lname: str
@param lname: Name of the link.
@param frame_name str: set reference frame name (from 315.2.5)
Expand All @@ -1397,10 +1434,7 @@ def getReferenceRotation(self, lname=None, frame_name=None):
\endverbatim
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getReferencePotation(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getReferenceRotation, frame_name)
pose = self.getReferencePose(lname, frame_name)
return [pose[0:3], pose[4:7], pose[8:11]]

Expand All @@ -1409,17 +1443,17 @@ def getReferenceRPY(self, lname=None, frame_name=None):
Returns the current commanded rotation in RPY of the specified joint.
cf. getCurrentRPY that returns physical value.
For lname, when either None passed / no lname is specified, all
eefs will be iterated and *only print* the result (i.e. nothing
returned). See _get_geometry for more info.
@type lname: str
@param lname: Name of the link.
@param frame_name str: set reference frame name (from 315.2.5)
@rtype: list of float
@return: List of orientation in rpy form about the specified joint.
'''
if not lname:
for item in self.Groups:
eef_name = item[1][-1]
print("{}: {}".format(eef_name, self.getReferenceRPY(eef_name)))
raise RuntimeError("need to specify joint name")
self._get_geometry(self.getReferenceRPY, frame_name)
return euler_from_matrix(self.getReferenceRotation(lname, frame_name), 'sxyz')

def setTargetPose(self, gname, pos, rpy, tm, frame_name=None):
Expand Down

0 comments on commit fc022e8

Please sign in to comment.