Skip to content

[fix][Python] link name's default value for get* methods. #1118

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
130s wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[fix][Python] link name's default value for get* methods. #1118

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e161aed
[Python] Fix link name's default value for get* methods.
130s Mar 16, 2017
71024d3
[Python] Extract common procedure for get* geometry methods.
130s Mar 16, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
110 changes: 72 additions & 38 deletions python/hrpsys_config.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1208,6 +1208,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 @@ -1248,10 +1285,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 @@ -1272,25 +1306,28 @@ 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 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]]

def getCurrentRotation(self, lname, frame_name=None):
def getCurrentRotation(self, lname=None, frame_name=None):
'''!@brief
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 @@ -1304,36 +1341,36 @@ def getCurrentRotation(self, lname, 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]]

def getCurrentRPY(self, lname, frame_name=None):
def getCurrentRPY(self, lname=None, frame_name=None):
'''!@brief
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, frame_name=None):
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 @@ -1348,10 +1385,7 @@ def getReferencePose(self, lname, 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 @@ -1361,11 +1395,14 @@ def getReferencePose(self, lname, frame_name=None):
raise RuntimeError("Could not find reference : " + lname)
return pose[1].data

def getReferencePosition(self, lname, frame_name=None):
def getReferencePosition(self, lname=None, frame_name=None):
'''!@brief
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 @@ -1374,18 +1411,18 @@ def getReferencePosition(self, lname, 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]]

def getReferenceRotation(self, lname, frame_name=None):
def getReferenceRotation(self, lname=None, frame_name=None):
'''!@brief
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 @@ -1399,29 +1436,26 @@ def getReferenceRotation(self, lname, 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]]

def getReferenceRPY(self, lname, frame_name=None):
def getReferenceRPY(self, lname=None, frame_name=None):
'''!@brief
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