Skip to content
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

Jump to bottom

[hironx client] Remove experimental dependency #470

Open
wants to merge 2 commits into
base: indigo-devel
Choose a base branch
from
Open

[hironx client] Remove experimental dependency #470

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a40acc2
[hironx client] Remove experimental dependency (see https://github.co…
130s Oct 12, 2016
adc378e
[test] Fix local var referenced before assignment issue (see https://…
130s Oct 15, 2016
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
137 changes: 2 additions & 135 deletions hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,141 +84,8 @@ def delete_module(modname, paranoid=None):
except AttributeError:
pass

class HrpsysConfigurator2(HrpsysConfigurator): ## JUST FOR TEST, REMOVE WHEN YOU MERGE
default_frame_name = 'WAIST'

def getCurrentPose(self, lname=None, frame_name=None):
'''!@brief
Returns the current physical pose of the specified joint.
cf. getReferencePose that returns commanded value.

eg.
\verbatim
IN: robot.getCurrentPose('LARM_JOINT5')
OUT: [-0.0017702356144599085,
0.00019034630541264752,
-0.9999984150158207,
0.32556275164378523,
0.00012155879975329215,
0.9999999745367515,
0.0001901314142046251,
0.18236394191140365,
0.9999984257434246,
-0.00012122202968358842,
-0.001770258707652326,
0.07462472659364472,
0.0,
0.0,
0.0,
1.0]
\endverbatim

@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: Rotational matrix and the position of the given joint in
1-dimensional list, that is:
\verbatim
[a11, a12, a13, x,
a21, a22, a23, y,
a31, a32, a33, z,
0, 0, 0, 1]
\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")
####
#### for hrpsys >= 315.2.5, frame_name is supported
#### default_frame_name is set, call with lname:default_frame_name
#### frame_name is given, call with lname:frame_name
#### frame_name is none, call with lname
#### for hrpsys <= 315.2.5, frame_name is not supported
#### frame_name is given, call with lname with warning / oerror
#### frame_name is none, call with lname
if StrictVersion(self.fk_version) >= StrictVersion('315.2.5'): ### CHANGED
if self.default_frame_name and frame_name is None:
frame_name = self.default_frame_name
if frame_name and not ':' in lname:
lname = lname + ':' + frame_name
else: # hrpsys < 315.2.4
if frame_name:
print('frame_name ('+lname+') is not supported') ### CHANGED
pose = self.fk_svc.getCurrentPose(lname)
if not pose[0]:
raise RuntimeError("Could not find reference : " + lname)
return pose[1].data

def getReferencePose(self, lname, frame_name=None):
'''!@brief
Returns the current commanded pose of the specified joint.
cf. getCurrentPose that returns physical pose.

@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: Rotational matrix and the position of the given joint in
1-dimensional list, that is:
\verbatim
[a11, a12, a13, x,
a21, a22, a23, y,
a31, a32, a33, z,
0, 0, 0, 1]
\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")
if StrictVersion(self.fk_version) >= StrictVersion('315.2.5'): ### CHANGED
if self.default_frame_name and frame_name is None:
frame_name = self.default_frame_name
if frame_name and not ':' in lname:
lname = lname + ':' + frame_name
else: # hrpsys < 315.2.4
if frame_name:
print('frame_name ('+lname+') is not supported') ### CHANGED
pose = self.fk_svc.getReferencePose(lname)
if not pose[0]:
raise RuntimeError("Could not find reference : " + lname)
return pose[1].data

def setTargetPose(self, gname, pos, rpy, tm, frame_name=None):
'''!@brief
Move the end-effector to the given absolute pose.
All d* arguments are in meter.

@param gname str: Name of the joint group.
@param pos list of float: In meter.
@param rpy list of float: In radian.
@param tm float: Second to complete the command.
@param frame_name str: Name of the frame that this particular command
references to.
@return bool: False if unreachable.
'''
print(gname, frame_name, pos, rpy, tm)
if StrictVersion(self.seq_version) >= StrictVersion('315.2.5'): ### CHANGED
if self.default_frame_name and frame_name is None:
frame_name = self.default_frame_name
if frame_name and not ':' in gname:
gname = gname + ':' + frame_name
else: # hrpsys < 315.2.4
if frame_name and not ':' in gname:
print('frame_name ('+gname+') is not supported') ### CHANGED
result = self.seq_svc.setTargetPose(gname, pos, rpy, tm)
if not result:
print("setTargetPose failed. Maybe SequencePlayer failed to solve IK.\n"
+ "Currently, returning IK result error\n"
+ "(like the one in https://github.com/start-jsk/rtmros_hironx/issues/103)"
+ " is not implemented. Patch is welcomed.")
return result

class HIRONX(HrpsysConfigurator2):

class HIRONX(HrpsysConfigurator):
'''
@see: <a href = "https://github.com/fkanehiro/hrpsys-base/blob/master/" +
"python/hrpsys_config.py">HrpsysConfigurator</a>
Expand Down
1 change: 1 addition & 0 deletions hironx_ros_bridge/test/test_hironx_target.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,7 @@ def print_pose(msg, pose):
self.robot.goInitial()

posel1 = self.robot.getCurrentPose('LARM_JOINT5')
posel2 = None
try:
posel2 = self.robot.getCurrentPose('LARM_JOINT5', 'WAIST')
except RuntimeError as e:
Expand Down