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

add test for getJointAnglesOfGroup #530

Open
wants to merge 3 commits into
base: indigo-devel
Choose a base branch
from
+16 −4
Open

add test for getJointAnglesOfGroup #530

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
49696ab
add test for getJointAnglesOfGroup
k-okada Jul 24, 2018
c18fc4d
fix error message
k-okada Jul 24, 2018
c8555e3
fix getJointAnglesOfGroup, not sure why #505 is not tested
k-okada Jul 24, 2018
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
10 changes: 6 additions & 4 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 @@ -1104,20 +1104,22 @@ def getJointAnglesOfGroup(self, limb):
offset = 0
if len(angles) != reduce(lambda x,y: x+len(y[1]), self.Groups, 0):
offset = 4
angles = []
group_angles = []
index = 0
if len(angles) != reduce(lambda x,y: x+len(y[1]), self.Groups, 0) + offset + offset:
index = 1
for group in self.Groups:
joint_num = len(group[1])
angles.append(angles[index: index + joint_num])
group_angles.append(angles[index: index + joint_num])
index += joint_num
if group[0] in ['larm', 'rarm']:
index += offset ## FIX ME
groups = self.Groups
for i in range(len(groups)):
if groups[i][0] == limb:
return angles[i]
return group_angles[i]
print self.configurator_name, 'could not find limb name ' + limb
print self.configurator_name, ' in' + filter(lambda x: x[0], groups)
print self.configurator_name, ' in ' + str(map(lambda x: x[0], groups))

def clearOfGroup(self, limb):
'''!@brief
Expand Down
10 changes: 10 additions & 0 deletions hironx_ros_bridge/test/test_hironx_limb.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -199,6 +199,16 @@ def test_rarm_setJointAnglesOfGroup_minus(self):

self.robot.el_svc.setServoErrorLimit("all", 0.18) # default is 0.18

def test_getJointAnglesOfGroup(self):
self.robot.setJointAnglesOfGroup("torso", [-1.234], 3, wait=False)
self.robot.setJointAnglesOfGroup("head", [5.6789, -10.1112], 3, wait=False)
self.robot.setJointAnglesOfGroup("rarm", [-1.6, 0, -100, 15.2, 9.4,-3.2], 3, wait=False)
self.robot.setJointAnglesOfGroup("larm", [ 1.6, 0, -100,-15.2, 9.4, 3.2], 3, wait=True)
numpy.testing.assert_array_almost_equal([-1.234], self.robot.getJointAnglesOfGroup("torso"))
numpy.testing.assert_array_almost_equal([5.6789, -10.1112], self.robot.getJointAnglesOfGroup("head"))
numpy.testing.assert_array_almost_equal([-1.6, 0, -100, 15.2, 9.4,-3.2], self.robot.getJointAnglesOfGroup("rarm"))
numpy.testing.assert_array_almost_equal([ 1.6, 0, -100,-15.2, 9.4, 3.2], self.robot.getJointAnglesOfGroup("larm"))

def test_movejoints_neck_waist(self):
'''
Move neck and waist joints to the positional limit defined in
Expand Down