From 2651bae7490fba4f54910ab9214dca7c85f028cb Mon Sep 17 00:00:00 2001 From: Rachel Holladay Date: Fri, 4 Sep 2015 13:37:50 -0400 Subject: [PATCH 01/12] initial demo version --- config/natural_feature_weights.pickle | 57 -------- src/herbpy/action/rogue.py | 198 +++++--------------------- 2 files changed, 35 insertions(+), 220 deletions(-) delete mode 100644 config/natural_feature_weights.pickle diff --git a/config/natural_feature_weights.pickle b/config/natural_feature_weights.pickle deleted file mode 100644 index f2e53c9..0000000 --- a/config/natural_feature_weights.pickle +++ /dev/null @@ -1,57 +0,0 @@ -(dp1 -S'Distance' -p2 -cnumpy.core.multiarray -scalar -p3 -(cnumpy -dtype -p4 -(S'f8' -I0 -I1 -tRp5 -(I3 -S'<' -NNNI-1 -I-1 -I0 -tbS'% 2\xdep\x9e\xc6?' -tRp6 -sS'WristRelative' -p7 -g3 -(g5 -S'\xc7\xff\xa9^I;\xc1?' -tRp8 -sS'ShoulderRotation' -p9 -g3 -(g5 -S':d\xb0\xc5\xa85\xac?' -tRp10 -sS'WristAngle' -p11 -g3 -(g5 -S'\xc5\x9f\x91\x94\x7f\x8e\xaa?' -tRp12 -sS'ElbowRotation' -p13 -g3 -(g5 -S'\xeb\xcf\xb4\x10Y\xd4\xe4?' -tRp14 -sS'WristRotation' -p15 -g3 -(g5 -S'\xe0%\x9c\x11E7\xbf?' -tRp16 -sS'OcculsionScore' -p17 -g3 -(g5 -S'+\xa4\xa7\xbeS\xc5\xe6?' -tRp18 -s. \ No newline at end of file diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index 078019a..0aa991c 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -17,7 +17,7 @@ def Point(robot, focus, manip=None, render=False): This must be the right arm @param render Render tsr samples during planning """ - import offscreen_render + #Pointing at an object if type(focus) == openravepy.openravepy_int.KinBody: focus_trans = focus.GetTransform() @@ -47,8 +47,7 @@ def Point(robot, focus, manip=None, render=False): point_tsr = robot.tsrlibrary(None, 'point', focus_trans, manip) with prpy.viz.RenderTSRList(point_tsr, robot.GetEnv(), render=render): - robot.PlanToTSR(point_tsr, execute=True, - ranker=Naturalness(focus_trans, goal_name)) + robot.PlanToTSR(point_tsr, execute=True) robot.right_hand.MoveHand(f1=2.4, f2=0.8, f3=2.4, spread=3.14) @ActionMethod @@ -165,7 +164,7 @@ def Sweep(robot, start, end, manip=None, margin=0.3, render=True): robot.PlanToTSR(sweep_tsr, execute=True) @ActionMethod -def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, render=True): +def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, release=True, render=True): """ @param robot The robot performing the exhibit @param obj The object being exhibited @@ -175,6 +174,10 @@ def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, render=True): @param render Render tsr samples during planning """ + if manip is None: + manip = robot.GetActiveManipulator() + + preconfig = manip.GetDOFValues() robot.Grasp(obj) #Lift the object - write more tsrs @@ -192,7 +195,10 @@ def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, render=True): with prpy.viz.RenderTSRList(unlift_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(unlift_tsr, execute=True) - #TODO: possibly then release, openhand, retract. + if release: + robot.Release(obj) + manip.hand.OpenHand() + manip.PlanToConfiguration(preconfig) @ActionMethod def Nod(robot, word='yes'): @@ -200,20 +206,32 @@ def Nod(robot, word='yes'): @param robot The robot being used to nod @param word Shakes up and down for 'yes' and left and right for 'no' """ - before = robot.head.GetDOFValues() - - if word == 'yes': - first = numpy.array([ 0., -0.43588399]) #Down - second = numpy.array([ 0., 0.26307139]) #Up - elif word == 'no': - first = numpy.array([-0.45916359, 0.]) #Left - second = numpy.array([0.45916359, 0.]) #Right + + import time + + if word == 'no': + for i in xrange(2): + robot.head.Servo([ 1, 0]) + time.sleep(.2) + for i in xrange(4): + robot.head.Servo([-1, 0]) + time.sleep(.2) + for i in xrange(2): + robot.head.Servo([ 1, 0]) + time.sleep(.2) + elif word == 'yes': + for i in xrange(2): + robot.head.Servo([ 0, 1]) + time.sleep(.2) + for i in xrange(4): + robot.head.Servo([ 0, -1]) + time.sleep(.2) + for i in xrange(2): + robot.head.Servo([ 0, 1]) + time.sleep(.2) else: raise prpy.exceptions.PrPyException('Word Not Recognized') - robot.head.MoveTo(first) - robot.head.MoveTo(second) - robot.head.MoveTo(before) @ActionMethod def HaltHand(robot, manip=None): @@ -266,7 +284,7 @@ def HighFive(robot, manip=None, wait=7): hand.TareForceTorqueSensor() #Move into canonical high fiving position - Stop(robot, manip) + HaltHand(robot, manip) #TODO wait in till robot.right_hand.GetForceTorque() reads something @@ -304,149 +322,3 @@ def MiddleFinger(robot, manip=None): else: raise prpy.exceptions.PrPyException('The middle finger is only defined \ for the left and right arm.') - -class Naturalness(object): - def __init__(self, focus_trans, goal_name): - self.focus_trans = focus_trans - self.goal_name = goal_name - - #Set up sensor parameters - self.sensor_length = 640 - self.sensor_width = 480 - self.sensor_weight = 255 - self.scoreMask = self.weightedScoreArray() - - feature_path = FindCatkinResource('herbpy', - 'config/natural_feature_weights.pickle') - - try: - self.featureWeights = cPickle.load(open(feature_path, 'rb')) - except IOError as e: - raise ValueError('Failed loading pointing weights \ - from "{:s}".'.format(feature_path)) - - - def __call__(self, robot, ik_solutions): - self.robot = robot - self.env = self.robot.GetEnv() - self.manip = robot.GetActiveManipulator() - num_sols = ik_solutions.shape[0] - results = numpy.zeros(num_sols) - - #Create sensor - self.sensor = openravepy.RaveCreateSensor(self.env, - 'offscreen_render_camera') - self.sensor.SendCommand('setintrinsic 529 525 328 267 0.01 10') - self.sensor.SendCommand('setdims '+str(self.sensor_length)+ - ' '+str(self.sensor_width)) - self.sensor.Configure(openravepy.Sensor.ConfigureCommand.PowerOn) - resetDOFs = self.manip.GetDOFValues() - - #Score each configuration - for i in xrange(0, num_sols): - results[i] = self.score_pose(ik_solutions[i]) - - self.manip.SetDOFValues(resetDOFs) - return results - - def score_pose(self, ik_sol): - """Set the robot in the configuration and then total up, - for each scoring parameter, the weight of that parameter - times its score""" - self.manip.SetDOFValues(ik_sol) - self.pose = self.manip.GetEndEffectorTransform() - - total = 0 - total += self.compute_score('/right/j1', 'ShoulderRotation', 1.5, 2, 0) - total += self.compute_score('/right/j5', 'ElbowRotation', -0.5, 2, 1.5) - total += self.compute_score('/right/j6', 'WristAngle', 0, 1, 0) - total += self.compute_score('/right/j7', 'WristRotation', 0, 1, 0) - total += self.wristOffset('WristRelative') - total += self.objectDistance('Distance') - - #If the focus is a point in space, not an object, there - #is no concept of occulsion - if self.goal_name is not None: - total += self.score_occulsion() - return total - - def compute_score(self, joint, dict_name, optimal, full_range, offset): - """For the generic joint based parameter, score based on - who far the configuration differs from the 'optimal' """ - actual = self.robot.GetJoint(joint).GetValue(1)+(offset*numpy.pi) - optimal_val = (optimal+offset)*numpy.pi - full_range_val = full_range*numpy.pi - weight = self.featureWeights[dict_name] - return (abs(optimal_val - actual) / full_range_val)*weight - - def wristOffset(self, dict_name): - """For finding how far the rotation of the wrist is offset - from being horizontal, find the angle between z value of the - wrist pose and the j unit vector. Then score by how much it - differs from the optimal, being horizontal.""" - z = self.pose[0:3, 2] - z_length = numpy.sqrt(z[0]**2 + z[1]**2 + z[2]**2) - angle = (numpy.arccos(z[1] / z_length)) / numpy.pi - optimal = 0 - full_range = 2 - offset = abs(optimal - angle) / full_range - return self.featureWeights[dict_name] * offset - - def objectDistance(self, dict_name): - """Compute the distance between the end effector and the object.""" - [px, py, pz] = self.pose[0:3, 3] - [gx, gy, gz] = self.focus_trans[0:3, 3] - dist = numpy.sqrt((px-gx)**2 + (py - gy)**2 + (pz - gz)**2) - return self.featureWeights[dict_name]*dist - - def weightedScoreArray(self): - """The occulsion value is scored against a weighted array that - weights items closer to the center, i.e near the point's focus - higher then the periphery.""" - l = (self.sensor_length - 1) / 2 - w = (self.sensor_width - 1) /2 - w = numpy.fromfunction(lambda i, j: 1 / (1+numpy.sqrt((l-i)**2 - +(w-j)**2)), (self.sensor_length, self.sensor_width), dtype=int) - return numpy.ravel(w) - - def valFromPhoto(self, img): - """Takes the image generated by sensor and computes - the weighted score.""" - data = img.imagedata - compact_array = numpy.apply_over_axes(numpy.sum, data, [2]) - reshape_array = numpy.ravel(compact_array) - divide_out = reshape_array / self.sensor_weight - goal = numpy.dot(self.scoreMask, divide_out) - return goal - - def score_occulsion(self): - """Sensor captures base photo with just the goal object - and scores it against a photo with all objects in the scene - overlaid on the base photo.""" - pose = self.manip.GetEndEffectorTransform() - self.sensor.SetTransform(pose) - - self.sensor.SendCommand('addbody '+self.goal_name+' '+ - str(self.sensor_weight)+' 0 0') - self.sensor.SimulationStep(0.01) - data_solo = self.sensor.GetSensorData() - solo_goal = self.valFromPhoto(data_solo) - - #Get all other objects in the scene and add them to the image - allObjs = self.env.GetBodies() - for j in allObjs: - name = j.GetName() - if ((name != self.goal_name) and (name != self.robot.GetName())): - self.sensor.SendCommand('addbody '+name+' 0 0 0 0') - - self.sensor.SimulationStep(0.01) - data_all = self.sensor.GetSensorData() - all_goal = self.valFromPhoto(data_all) - - if solo_goal == 0: - score = 0 - else: - score = (float(solo_goal - all_goal) / float(solo_goal)) - - self.sensor.SendCommand('clearbodies') - return score*self.featureWeights['OcculsionScore'] From 5e62c5e0c0d1c999549edd07ccea6d1093438f1a Mon Sep 17 00:00:00 2001 From: Rachel Holladay Date: Fri, 4 Sep 2015 18:20:02 -0400 Subject: [PATCH 02/12] added init lines and real robot tweaks --- src/herbpy/action/__init__.py | 1 + src/herbpy/action/rogue.py | 27 ++++++++++++++------------- src/herbpy/tsr/__init__.py | 1 + 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/herbpy/action/__init__.py b/src/herbpy/action/__init__.py index c4d853d..c412ca9 100755 --- a/src/herbpy/action/__init__.py +++ b/src/herbpy/action/__init__.py @@ -1 +1,2 @@ from grasping import PushGrasp, Grasp +from rogue import * diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index 0aa991c..1fc631b 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -201,34 +201,35 @@ def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, release=True, render=T manip.PlanToConfiguration(preconfig) @ActionMethod -def Nod(robot, word='yes'): +def Nod(robot, word='yes', inc=4): """ @param robot The robot being used to nod @param word Shakes up and down for 'yes' and left and right for 'no' """ import time + pause = 0.15 if word == 'no': - for i in xrange(2): + for i in xrange(inc): robot.head.Servo([ 1, 0]) - time.sleep(.2) - for i in xrange(4): + time.sleep(pause) + for i in xrange((inc*3)): robot.head.Servo([-1, 0]) - time.sleep(.2) - for i in xrange(2): + time.sleep(pause) + for i in xrange((inc*2)): robot.head.Servo([ 1, 0]) - time.sleep(.2) + time.sleep(pause) elif word == 'yes': - for i in xrange(2): + for i in xrange(inc): robot.head.Servo([ 0, 1]) - time.sleep(.2) - for i in xrange(4): + time.sleep(pause) + for i in xrange((inc*3)): robot.head.Servo([ 0, -1]) - time.sleep(.2) - for i in xrange(2): + time.sleep(pause) + for i in xrange((inc*2)): robot.head.Servo([ 0, 1]) - time.sleep(.2) + time.sleep(pause) else: raise prpy.exceptions.PrPyException('Word Not Recognized') diff --git a/src/herbpy/tsr/__init__.py b/src/herbpy/tsr/__init__.py index 2bec163..3523343 100644 --- a/src/herbpy/tsr/__init__.py +++ b/src/herbpy/tsr/__init__.py @@ -7,3 +7,4 @@ from pitcher import * from tray import * from pop_tarts import * +from generic import * From 1a681fc16a32cea64f5ed2acff94a74472f34177 Mon Sep 17 00:00:00 2001 From: Rachel Holladay Date: Fri, 11 Sep 2015 15:04:15 -0400 Subject: [PATCH 03/12] wave gesture --- config/waveTrajs/wave0.xml | 10 ++++++++++ config/waveTrajs/wave1.xml | 10 ++++++++++ config/waveTrajs/wave2.xml | 10 ++++++++++ config/waveTrajs/wave3.xml | 10 ++++++++++ src/herbpy/action/rogue.py | 24 ++++++++++++++++++++++++ 5 files changed, 64 insertions(+) create mode 100644 config/waveTrajs/wave0.xml create mode 100644 config/waveTrajs/wave1.xml create mode 100644 config/waveTrajs/wave2.xml create mode 100644 config/waveTrajs/wave3.xml diff --git a/config/waveTrajs/wave0.xml b/config/waveTrajs/wave0.xml new file mode 100644 index 0000000..b47a2ea --- /dev/null +++ b/config/waveTrajs/wave0.xml @@ -0,0 +1,10 @@ + + + + + + + +5.03348748 -1.57569674 1.68788069 2.06769058 -1.66834313 1.536798209999999 0.2117534199999996 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 2.06769058 -1.66834313 1.536798209999999 0.2117534199999996 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 2.32312231 -1.66834313 1.53679821 0.2117534199999998 0 0 0 1.010805085068333 -8.786841625753019e-16 8.786841625753019e-16 8.786841625753019e-16 0.5054025425341667 5.03348748 -1.57569674 1.68788069 2.57855404 -1.66834313 1.53679821 0.21175342 0 0 0 0 0 0 0 0.5054025425341667 + + diff --git a/config/waveTrajs/wave1.xml b/config/waveTrajs/wave1.xml new file mode 100644 index 0000000..1b2ffd6 --- /dev/null +++ b/config/waveTrajs/wave1.xml @@ -0,0 +1,10 @@ + + + + + + + +5.03348748 -1.57569674 1.68788069 2.57855404 -1.66834313 1.53679821 0.2117534199999996 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 2.57855404 -1.66834313 1.53679821 0.2117534199999996 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 2.101471485 -1.66834313 1.53679821 0.2117534199999998 0 0 0 -1.38142325881679 -3.214722258480227e-16 3.214722258480227e-16 5.223923670030369e-16 0.6907116294083951 5.03348748 -1.57569674 1.68788069 1.62438893 -1.66834313 1.53679821 0.21175342 0 0 0 0 0 0 0 0.6907116294083951 + + diff --git a/config/waveTrajs/wave2.xml b/config/waveTrajs/wave2.xml new file mode 100644 index 0000000..bfb83bc --- /dev/null +++ b/config/waveTrajs/wave2.xml @@ -0,0 +1,10 @@ + + + + + + + +5.03348748 -1.57569674 1.68788069 1.62438893 -1.66834313 1.536798209999999 0.21175342 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 1.62438893 -1.66834313 1.536798209999999 0.21175342 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 2.101471485 -1.66834313 1.536798209999999 0.21175342 0 0 0 1.38142325881679 0 9.644166775440681e-16 -4.018402823100284e-17 0.6907116294083951 5.03348748 -1.57569674 1.68788069 2.57855404 -1.66834313 1.53679821 0.21175342 0 0 0 0 0 0 0 0.6907116294083951 + + diff --git a/config/waveTrajs/wave3.xml b/config/waveTrajs/wave3.xml new file mode 100644 index 0000000..6081f3a --- /dev/null +++ b/config/waveTrajs/wave3.xml @@ -0,0 +1,10 @@ + + + + + + + +5.03348748 -1.57569674 1.68788069 2.57855404 -1.66834313 1.53679821 0.2117534199999996 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 2.57855404 -1.66834313 1.53679821 0.2117534199999996 0 0 0 0 0 0 0 0 5.03348748 -1.57569674 1.68788069 2.32312231 -1.66834313 1.53679821 0.2117534199999998 0 0 0 -1.010805085068333 -4.393420812876509e-16 4.393420812876509e-16 7.139308820924327e-16 0.5054025425341667 5.03348748 -1.57569674 1.68788069 2.06769058 -1.66834313 1.53679821 0.21175342 0 0 0 0 0 0 0 0.5054025425341667 + + diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index 1fc631b..c964c16 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -323,3 +323,27 @@ def MiddleFinger(robot, manip=None): else: raise prpy.exceptions.PrPyException('The middle finger is only defined \ for the left and right arm.') + +@ActionMethod +def Wave(robot): + """ + @param robot The robot waving with their right arm + """ + + import prpy.rave + env = robot.GetEnv() + + manip = robot.right_arm + manip.SetActive() + print "Setting right arm as the active manipulator" + + traj0 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave0.xml') + traj1 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave1.xml') + traj2 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave2.xml') + traj3 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave3.xml') + + robot.HaltHand(manip=manip) + robot.ExecuteTrajectory(traj0) + robot.ExecuteTrajectory(traj1) + robot.ExecuteTrajectory(traj2) + robot.ExecuteTrajectory(traj3) From 189b5e11f13be186f074d23c721f17b402b62633 Mon Sep 17 00:00:00 2001 From: Rachel Holladay Date: Fri, 11 Sep 2015 15:28:41 -0400 Subject: [PATCH 04/12] more robust file finding --- scripts/gesture_control.py | 15 +++++++++++++++ src/herbpy/action/rogue.py | 16 +++++++++++----- 2 files changed, 26 insertions(+), 5 deletions(-) create mode 100644 scripts/gesture_control.py diff --git a/scripts/gesture_control.py b/scripts/gesture_control.py new file mode 100644 index 0000000..22f7cdd --- /dev/null +++ b/scripts/gesture_control.py @@ -0,0 +1,15 @@ +import herbpy +import numpy + +env, robot = herbpy.initialize(attach_viewer=True) + +keep_going = True + +while keep_going: + print "Press W to wave\nPress Q to quit.\n" + user_input = raw_input("Gesture? ") + + if user_input == 'Q': + keep_going = False + elif user_input == 'W': + robot.Wave() diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index c964c16..eed87c4 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -330,17 +330,23 @@ def Wave(robot): @param robot The robot waving with their right arm """ - import prpy.rave + from prpy.rave import load_trajectory + from prpy.util import FindCatkinResource env = robot.GetEnv() manip = robot.right_arm manip.SetActive() print "Setting right arm as the active manipulator" - traj0 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave0.xml') - traj1 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave1.xml') - traj2 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave2.xml') - traj3 = prpy.rave.load_trajectory(env, 'herbpy/config/waveTrajs/wave3.xml') + wave_path = FindCatkinResource('herbpy', 'config/waveTrajs/') + try: + traj0 = load_trajectory(env, wave_path+'wave0.xml') + traj1 = load_trajectory(env, wave_path+'wave1.xml') + traj2 = load_trajectory(env, wave_path+'wave2.xml') + traj3 = load_trajectory(env, wave_path+'wave3.xml') + except IOError as e: + raise ValueError('Failed loading wave trajectory from "{:s}".'.format( + wave_path)) robot.HaltHand(manip=manip) robot.ExecuteTrajectory(traj0) From 68d4c394c00efccefc46f792446bcb875e5cfca3 Mon Sep 17 00:00:00 2001 From: Rachel Holladay Date: Wed, 16 Sep 2015 19:45:29 -0400 Subject: [PATCH 05/12] simple gesture control interface --- scripts/gesture_control.py | 62 ++++++++++++++++++++++++++++++++------ src/herbpy/action/rogue.py | 1 - 2 files changed, 53 insertions(+), 10 deletions(-) mode change 100644 => 100755 scripts/gesture_control.py diff --git a/scripts/gesture_control.py b/scripts/gesture_control.py old mode 100644 new mode 100755 index 22f7cdd..281ec97 --- a/scripts/gesture_control.py +++ b/scripts/gesture_control.py @@ -1,15 +1,59 @@ +#!/usr/bin/env python import herbpy import numpy -env, robot = herbpy.initialize(attach_viewer=True) +if __name__ == '__main__': -keep_going = True + env, robot = herbpy.initialize() + keep_going = True -while keep_going: - print "Press W to wave\nPress Q to quit.\n" - user_input = raw_input("Gesture? ") + while keep_going: + print "\n\nWelcome to the gesture control:\n \ + Press W to wave.\n \ + Press S to say something.\n \ + Press Y to nod 'yes'.\n \ + Press N to nod 'no'.\n \ + Press H to high five.\n \ + Press P to point at an object.\n \ + Press S to present an object.\n \ + Press R to reset the robot.\n \ + Press Q to quit.\n" + user_input = raw_input("Gesture? ") - if user_input == 'Q': - keep_going = False - elif user_input == 'W': - robot.Wave() + if user_input == 'Q': + print 'Goodbye!\n' + keep_going = False + elif user_input == 'W': + print 'Waving!\n' + robot.Wave() + elif user_input == 'Y': + print 'Nodding yes!\n' + robot.Nod(word='yes') + elif user_input == 'N': + print 'Nodding no\n!' + robot.Nod(word='no') + elif user_input == 'H': + print 'High Fiving!\n' + robot.HighFive() + elif user_input == 'R': + robot.right_arm.PlanToNamedConfiguration('relaxed_home', execute=True) + robot.left_arm.PlanToNamedConfiguration('relaxed_home', execute=True) + robot.right_hand.OpenHand() + robot.left_hand.OpenHand() + elif user_input == 'S': + say_this = raw_input('What do you want HERB to say? ') + robot.Say(str(say_this)) + elif user_input == 'P': + items = [x.GetName() for x in env.GetBodies()] + print "HERB can see these things:" + print items[1:] #Don't show the robot as an option + point_at = raw_input('I want to point at item number: ') + robot.Point(env.GetBodies()[int(point_at)]) + elif user_input == 'S': + items = [x.GetName() for x in env.GetBodies()] + print "HERB can see these things:" + print items[1:] + point_at = raw_input('I want to point at item number: ') + robot.Point(env.GetBodies()[int(point_at)]) + else: + print 'Input Not Recognized\n' diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index eed87c4..ece566a 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -30,7 +30,6 @@ def Point(robot, focus, manip=None, render=False): focus_trans = numpy.eye(4) focus_trans[0:3, 3] = focus goal_name = None - #Possible feature: if many objects then pass to multipoint else: raise prpy.exceptions.PrPyException('Focus of the point is an \ unknown object') From 61c9212ea610ad2dd6b4574fd135c6a9653b26a2 Mon Sep 17 00:00:00 2001 From: Rachel Date: Thu, 15 Oct 2015 12:00:40 -0400 Subject: [PATCH 06/12] removing high five --- src/herbpy/action/rogue.py | 33 --------------------------------- 1 file changed, 33 deletions(-) diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index ece566a..7a5de83 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -261,39 +261,6 @@ def HaltHand(robot, manip=None): raise prpy.exceptions.PrPyException('Stop is only defined \ for the left and right arm.') -@ActionMethod -def HighFive(robot, manip=None, wait=7): - """ - @param robot The robot being used to high five - @param manip The manipulator being used to high five - @param wait The time to hold the hand up after feeling the - force before the hand retracts - """ - if manip is None: - manip = robot.GetActiveManipulator() - - if manip.GetName() == 'left': - hand = robot.left_hand - elif manip.GetName() == 'right': - hand = robot.right_hand - else: - prpy.exceptions.PrPyException('High Fiving is only defined \ - for the left and right arm') - - before = manip.GetDOFValues() - hand.TareForceTorqueSensor() - - #Move into canonical high fiving position - HaltHand(robot, manip) - - #TODO wait in till robot.right_hand.GetForceTorque() reads something - - #After having felt force, wait a few seconds - time.sleep(wait) - - #Retract the arm to where it was before the interaction - manip.PlanToConfiguration(before) - @ActionMethod def MiddleFinger(robot, manip=None): """ From 52789646bd8fb446345142d752ff3f18cdd68157 Mon Sep 17 00:00:00 2001 From: Rachel Date: Thu, 15 Oct 2015 21:23:07 -0400 Subject: [PATCH 07/12] Gesture Code: Split up into helper functions, safer code with statesavers --- src/herbpy/action/rogue.py | 277 +++++++++++++++++++++---------------- src/herbpy/tsr/generic.py | 40 +++--- 2 files changed, 178 insertions(+), 139 deletions(-) diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index 7a5de83..7421acb 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -8,7 +8,7 @@ logger = logging.getLogger('herbpy') @ActionMethod -def Point(robot, focus, manip=None, render=False): +def PointAt(robot, focus, manip=None, render=False): """ @param robot The robot performing the point @param focus The 3-D coordinate in space or object @@ -17,40 +17,99 @@ def Point(robot, focus, manip=None, render=False): This must be the right arm @param render Render tsr samples during planning """ + env = robot.GetEnv() + pointing_coord = GetPointFrom(env, focus) + Point(robot, pointing_coord, manip, render) + +@ActionMethod +def PresentAt(robot, focus, manip=None, render=True): + """ + @param robot The robot performing the presentation + @param focus The 3-D coordinate in space or object that + is being presented + @param manip The manipulator to perform the presentation with. + This must be the right arm. + @param render Render tsr samples during planning + """ + env = robot.GetEnv() + presenting_coord = GetPointFrom(env, focus) + Present(robot, presenting_coord, manip, render) +@ActionMethod +def SweepAt(robot, start, end, manip=None, margin=0.3, render=True): + """ + @param robot The robot performing the sweep + @param start The object or 3-d position that marks the start + @param end The object of 3-d position that marks the end + @param manip The manipulator to perform the sweep + @param margin The distance between the start object and the hand, + so the vertical space between the hand and objects. + This must be enough to clear the objects themselves. + @param render Render tsr samples during planning + """ + env = robot.GetEnv() + start_coord = GetPointFrom(env, start) + end_coord = GetPointFrom(env, end) + Sweep(robot, start_coord, end_coord, manip, margin, render) + + +def GetPointFrom(env, focus): + """ + @param env The environment where the item exists + @param focus The area to be referred to + """ #Pointing at an object - if type(focus) == openravepy.openravepy_int.KinBody: - focus_trans = focus.GetTransform() - goal_name = focus.GetName() - #Pointing at a point in space - elif (type(focus) == numpy.ndarray) and (focus.ndim == 1): - if len(focus) != 3: - raise prpy.exceptions.PrPyExceptions('A point in space must \ - contain exactly three coordinates') - focus_trans = numpy.eye(4) - focus_trans[0:3, 3] = focus - goal_name = None + if isinstance(focus, openravepy.KinBody): + with env: + focus_trans = focus.GetTransform() + coord = list(focus_trans[0:3, 3]) + + #Pointing at a point in space as numpy array + elif (isinstance(focus, numpy.ndarray) and (focus.ndim == 1) + and (len(focus) == 3)): + coord = list(focus) + + #Pointing at a point in space as list + elif isinstance(focus, list): + coord = focus + + #Pointing at a point in space as tuple + elif isinstance(focus, tuple): + coord = list(focus) + else: raise prpy.exceptions.PrPyException('Focus of the point is an \ unknown object') + return coord + +def Point(robot, coord, manip=None, render=False): + """ + @param robot The robot performing the point + @param focus The 3-D coordinate in space that is being pointed at + @param manip The manipulator to perform the point with. + This must be the right arm + @param render Render tsr samples during planning + """ if manip is None: - print "Setting the right arm to be the active manipulator." manip = robot.right_arm - manip.SetActive() if manip.GetName() != 'right': raise prpy.exceptions.PrPyException('Pointing is only defined \ on the right arm.') + focus_trans = numpy.eye(4) + focus_trans[0:3, 3] = coord point_tsr = robot.tsrlibrary(None, 'point', focus_trans, manip) - with prpy.viz.RenderTSRList(point_tsr, robot.GetEnv(), render=render): - robot.PlanToTSR(point_tsr, execute=True) - robot.right_hand.MoveHand(f1=2.4, f2=0.8, f3=2.4, spread=3.14) + p = openravepy.KinBody.SaveParameters + with robot.CreateRobotStateSaver(p.ActiveManipulator): + robot.SetActiveManipulator(manip) + with prpy.viz.RenderTSRList(point_tsr, robot.GetEnv(), render=render): + robot.PlanToTSR(point_tsr, execute=True) + manip.hand.MoveHand(f1=2.4, f2=0.8, f3=2.4, spread=3.14) -@ActionMethod -def Present(robot, focus, manip=None, render=True): +def Present(robot, coord, manip=None, render=True): """ @param robot The robot performing the presentation @param focus The 3-D coordinate in space or object that @@ -59,38 +118,27 @@ def Present(robot, focus, manip=None, render=True): This must be the right arm. @param render Render tsr samples during planning """ - #Presenting an object - if type(focus) == openravepy.openravepy_int.KinBody: - focus_trans = focus.GetTransform() - #Presenting a point in space - elif (type(focus) == numpy.ndarray) and (focus.ndim == 1): - if len(focus) != 3: - raise prpy.exceptions.PrPyExceptions('A presentation in space \ - must contain exactly three coordinates') - focus_trans = numpy.eye(4) - focus_trans[0:3, 3] = focus - else: - raise prpy.exceptions.PrPyException('Focus of the presentation is an \ - unknown object') + focus_trans = numpy.eye(4) + focus_trans[0:3, 3] = coord + + present_tsr = robot.tsrlibrary(None, 'present', focus_trans, manip) if manip is None: - print "Setting the right arm to be the active manipulator." manip = robot.right_arm - manip.SetActive() if manip.GetName() != 'right': raise prpy.exceptions.PrPyException('Presenting is only defined \ on the right arm.') - - present_tsr = robot.tsrlibrary(None, 'present', focus_trans, manip) - - with prpy.viz.RenderTSRList(present_tsr, robot.GetEnv(), render=render): - robot.PlanToTSR(present_tsr, execute=True) - robot.right_hand.MoveHand(f1=1, f2=1, f3=1, spread=3.14) + p = openravepy.KinBody.SaveParameters + with robot.CreateRobotStateSaver(p.ActiveManipulator): + robot.SetActiveManipulator(manip) + with prpy.viz.RenderTSRList(present_tsr, robot.GetEnv(), render=render): + robot.PlanToTSR(present_tsr, execute=True) + manip.hand.MoveHand(f1=1, f2=1, f3=1, spread=3.14) -@ActionMethod -def Sweep(robot, start, end, manip=None, margin=0.3, render=True): + +def Sweep(robot, start_coords, end_coords, manip=None, margin=0.3, render=True): """ @param robot The robot performing the sweep @param start The object or 3-d position that marks the start @@ -101,33 +149,6 @@ def Sweep(robot, start, end, manip=None, margin=0.3, render=True): This must be enough to clear the objects themselves. @param render Render tsr samples during planning """ - - if type(start) == openravepy.openravepy_int.KinBody: - start_trans = start.GetTransform() - start_coords = start_trans[0:3, 3] - elif (type(start) == numpy.ndarray) and (start.ndim == 1): - if len(start) != 3: - raise prpy.exceptions.PrPyExceptions('A point in space must \ - contain exactly three coordinates') - start_coords = start - else: - raise prpy.exceptions.PrPyException('Focus of the point is an \ - unknown object') - - if type(end) == openravepy.openravepy_int.KinBody: - end_trans = end.GetTransform() - end_coords = end_trans[0:3, 3] - elif (type(end) == numpy.ndarray) and (end.ndim == 1): - if len(end) != 3: - raise prpy.exceptions.PrPyExceptions('A point in space must \ - contain exactly three coordinates') - end_coords = end - end_trans = numpy.eye(4) - end_trans[0:3, 3] = end_coords - else: - raise prpy.exceptions.PrPyException('Focus of the point is an \ - unknown object') - if manip is None: manip = robot.GetActiveManipulator() @@ -153,14 +174,20 @@ def Sweep(robot, start, end, manip=None, margin=0.3, render=True): raise prpy.exceptions.PrPyException('Manipulator does not have an \ associated hand') + end_trans = numpy.eye(4) + end_trans[0:3, 3] = end_coords + hand.MoveHand(f1=1, f2=1, f3=1, spread=3.14) manip.PlanToEndEffectorPose(hand_pose) #TSR to sweep to end position sweep_tsr = robot.tsrlibrary(None, 'sweep', end_trans, manip) - with prpy.viz.RenderTSRList(sweep_tsr, robot.GetEnv(), render=render): - robot.PlanToTSR(sweep_tsr, execute=True) + p = openravepy.KinBody.SaveParameters + with robot.CreateRobotStateSaver(p.ActiveManipulator): + robot.SetActiveManipulator(manip) + with prpy.viz.RenderTSRList(sweep_tsr, robot.GetEnv(), render=render): + robot.PlanToTSR(sweep_tsr, execute=True) @ActionMethod def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, release=True, render=True): @@ -172,12 +199,11 @@ def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, release=True, render=T @param wait The amount of time the object will be held up in seconds @param render Render tsr samples during planning """ - - if manip is None: - manip = robot.GetActiveManipulator() - - preconfig = manip.GetDOFValues() - robot.Grasp(obj) + with robot.GetEnv(): + if manip is None: + manip = robot.GetActiveManipulator() + preconfig = manip.GetDOFValues() + robot.Grasp(obj) #Lift the object - write more tsrs lift_tsr = robot.tsrlibrary(obj, 'lift', manip, distance=distance) @@ -195,43 +221,48 @@ def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, release=True, render=T robot.PlanToTSR(unlift_tsr, execute=True) if release: - robot.Release(obj) + with robot.GetEnv(): + robot.Release(obj) manip.hand.OpenHand() manip.PlanToConfiguration(preconfig) @ActionMethod -def Nod(robot, word='yes', inc=4): +def NodYes(robot): """ @param robot The robot being used to nod - @param word Shakes up and down for 'yes' and left and right for 'no' """ - import time pause = 0.15 + inc = 4 + + for i in xrange(inc): + robot.head.Servo([ 0, 1]) + time.sleep(pause) + for i in xrange((inc*3)): + robot.head.Servo([ 0, -1]) + time.sleep(pause) + for i in xrange((inc*2)): + robot.head.Servo([ 0, 1]) + time.sleep(pause) - if word == 'no': - for i in xrange(inc): - robot.head.Servo([ 1, 0]) - time.sleep(pause) - for i in xrange((inc*3)): - robot.head.Servo([-1, 0]) - time.sleep(pause) - for i in xrange((inc*2)): - robot.head.Servo([ 1, 0]) - time.sleep(pause) - elif word == 'yes': - for i in xrange(inc): - robot.head.Servo([ 0, 1]) - time.sleep(pause) - for i in xrange((inc*3)): - robot.head.Servo([ 0, -1]) - time.sleep(pause) - for i in xrange((inc*2)): - robot.head.Servo([ 0, 1]) - time.sleep(pause) - else: - raise prpy.exceptions.PrPyException('Word Not Recognized') - +@ActionMethod +def NodNo(robot): + """ + @param robot The robot being used to nod + """ + import time + pause = 0.15 + inc = 4 + + for i in xrange(inc): + robot.head.Servo([ 1, 0]) + time.sleep(pause) + for i in xrange((inc*3)): + robot.head.Servo([-1, 0]) + time.sleep(pause) + for i in xrange((inc*2)): + robot.head.Servo([ 1, 0]) + time.sleep(pause) @ActionMethod def HaltHand(robot, manip=None): @@ -248,14 +279,14 @@ def HaltHand(robot, manip=None): 2.06769058, -1.66834313, 1.53679821, 0.21175342]) - manip.PlanToConfiguration(pose) + manip.PlanToConfiguration(pose, execute=True) robot.right_hand.MoveHand(f1=0, f2=0, f3=0, spread=3.14) elif manip.GetName() == 'left': pose = numpy.array([ 1.30614268, -1.76 , -1.57063853, 2.07228362, 1.23918377, 1.46215605, -0.12918424]) - manip.PlanToConfiguration(pose) + manip.PlanToConfiguration(pose, execute=True) robot.left_hand.MoveHand(f1=0, f2=0, f3=0, spread=3.14) else: raise prpy.exceptions.PrPyException('Stop is only defined \ @@ -290,6 +321,7 @@ def MiddleFinger(robot, manip=None): raise prpy.exceptions.PrPyException('The middle finger is only defined \ for the left and right arm.') + @ActionMethod def Wave(robot): """ @@ -298,24 +330,25 @@ def Wave(robot): from prpy.rave import load_trajectory from prpy.util import FindCatkinResource + from os.path import join env = robot.GetEnv() - manip = robot.right_arm - manip.SetActive() - print "Setting right arm as the active manipulator" - wave_path = FindCatkinResource('herbpy', 'config/waveTrajs/') try: - traj0 = load_trajectory(env, wave_path+'wave0.xml') - traj1 = load_trajectory(env, wave_path+'wave1.xml') - traj2 = load_trajectory(env, wave_path+'wave2.xml') - traj3 = load_trajectory(env, wave_path+'wave3.xml') + traj0 = load_trajectory(env, join(wave_path, 'wave0.xml')) + traj1 = load_trajectory(env, join(wave_path, 'wave1.xml')) + traj2 = load_trajectory(env, join(wave_path, 'wave2.xml')) + traj3 = load_trajectory(env, join(wave_path, 'wave3.xml')) except IOError as e: - raise ValueError('Failed loading wave trajectory from "{:s}".'.format( - wave_path)) - - robot.HaltHand(manip=manip) - robot.ExecuteTrajectory(traj0) - robot.ExecuteTrajectory(traj1) - robot.ExecuteTrajectory(traj2) - robot.ExecuteTrajectory(traj3) + raise IOError(str(e)) + + p = openravepy.KinBody.SaveParameters + with robot.CreateRobotStateSaver(p.ActiveManipulator): + manip = robot.right_arm + robot.SetActiveManipulator(manip) + + robot.HaltHand(manip=manip) + robot.ExecuteTrajectory(traj0) + robot.ExecuteTrajectory(traj1) + robot.ExecuteTrajectory(traj2) + robot.ExecuteTrajectory(traj3) diff --git a/src/herbpy/tsr/generic.py b/src/herbpy/tsr/generic.py index 9d643ea..15fa308 100644 --- a/src/herbpy/tsr/generic.py +++ b/src/herbpy/tsr/generic.py @@ -9,12 +9,15 @@ def point_obj(robot, transform, manip=None): @param transform The location of where the robot is pointing to @param manip The manipulator to point with. This must be the right arm. """ - if manip is None: - manip = robot.right_arm - with manip.GetRobot(): - manip.SetActive() - manip_idx = manip.GetRobot().GetActiveManipulatorIndex() + with robot.GetEnv(): + if manip is None: + manip = robot.right_arm + + with robot.CreateRobotStateSaver( + robot.SaveParameters.ActiveManipulator): + robot.SetActiveManipulator(manip) + manip_idx = manip.GetRobot().GetActiveManipulatorIndex() if manip.GetName() != 'right': raise prpy.exceptions.PrPyException('Pointing is only defined on the right arm.') @@ -56,12 +59,14 @@ def present_obj(robot, transform, manip=None): @param manip The manipulator to present. This must be the right arm. """ - if manip is None: - manip = robot.right_arm + with robot.GetEnv(): + if manip is None: + manip = robot.right_arm - with manip.GetRobot(): - manip.SetActive() - manip_idx = manip.GetRobot().GetActiveManipulatorIndex() + with robot.CreateRobotStateSaver( + robot.SaveParameters.ActiveManipulator): + robot.SetActiveManipulator(manip) + manip_idx = manip.GetRobot().GetActiveManipulatorIndex() if manip.GetName() != 'right': raise prpy.exceptions.PrpyException('Presenting is only defined for the right arm.') @@ -96,13 +101,14 @@ def sweep_objs(robot, transform, manip=None): @param manip The manipulator to sweep. """ - if manip is None: - manip = robot.GetActiveManipulator() - manip_idx = robot.GetActiveManipulatorIndex() - else: - with manip.GetRobot(): - manip.SetActive() - manip_idx = manip.GetRobot().GetActiveManipulatorIndex() + with robot.GetEnv(): + if manip is None: + manip = robot.right_arm + + with robot.CreateRobotStateSaver( + robot.SaveParameters.ActiveManipulator): + robot.SetActiveManipulator(manip) + manip_idx = manip.GetRobot().GetActiveManipulatorIndex() #TSR for the goal ee_offset = 0.2 From 76c3dc65917746e1c563d4c8bc0d5b1f2abac5f6 Mon Sep 17 00:00:00 2001 From: Rachel Date: Fri, 16 Oct 2015 21:24:41 -0400 Subject: [PATCH 08/12] clean up of state saver and python consistency --- src/herbpy/action/rogue.py | 85 ++++++++++++++++++++------------------ 1 file changed, 45 insertions(+), 40 deletions(-) diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index 7421acb..2e6e5fa 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -19,7 +19,7 @@ def PointAt(robot, focus, manip=None, render=False): """ env = robot.GetEnv() pointing_coord = GetPointFrom(env, focus) - Point(robot, pointing_coord, manip, render) + return Point(robot, pointing_coord, manip, render) @ActionMethod def PresentAt(robot, focus, manip=None, render=True): @@ -33,7 +33,7 @@ def PresentAt(robot, focus, manip=None, render=True): """ env = robot.GetEnv() presenting_coord = GetPointFrom(env, focus) - Present(robot, presenting_coord, manip, render) + return Present(robot, presenting_coord, manip, render) @ActionMethod def SweepAt(robot, start, end, manip=None, margin=0.3, render=True): @@ -50,7 +50,7 @@ def SweepAt(robot, start, end, manip=None, margin=0.3, render=True): env = robot.GetEnv() start_coord = GetPointFrom(env, start) end_coord = GetPointFrom(env, end) - Sweep(robot, start_coord, end_coord, manip, margin, render) + return Sweep(robot, start_coord, end_coord, manip, margin, render) def GetPointFrom(env, focus): @@ -59,7 +59,7 @@ def GetPointFrom(env, focus): @param focus The area to be referred to """ #Pointing at an object - if isinstance(focus, openravepy.KinBody): + if isinstance(focus, (openravepy.KinBody, openravepy.KinBody.Link)): with env: focus_trans = focus.GetTransform() coord = list(focus_trans[0:3, 3]) @@ -69,13 +69,13 @@ def GetPointFrom(env, focus): and (len(focus) == 3)): coord = list(focus) - #Pointing at a point in space as list - elif isinstance(focus, list): - coord = focus + #Pointing at point in space as 4x4 transform + elif isinstance(focus, numpy.ndarray) and (focus.shape == (4, 4)): + coord = list(focus[0:3, 3]) - #Pointing at a point in space as tuple - elif isinstance(focus, tuple): - coord = list(focus) + #Pointing at a point in space as list or tuple + elif (isinstance(focus, (tuple, list)) and len(focus) == 3): + coord = focus else: raise prpy.exceptions.PrPyException('Focus of the point is an \ @@ -98,13 +98,14 @@ def Point(robot, coord, manip=None, render=False): raise prpy.exceptions.PrPyException('Pointing is only defined \ on the right arm.') - focus_trans = numpy.eye(4) + focus_trans = numpy.eye(4, dtype='float') focus_trans[0:3, 3] = coord point_tsr = robot.tsrlibrary(None, 'point', focus_trans, manip) p = openravepy.KinBody.SaveParameters - with robot.CreateRobotStateSaver(p.ActiveManipulator): + with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): robot.SetActiveManipulator(manip) + robot.SetActiveDOF(manip.GetArmIndices) with prpy.viz.RenderTSRList(point_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(point_tsr, execute=True) manip.hand.MoveHand(f1=2.4, f2=0.8, f3=2.4, spread=3.14) @@ -118,21 +119,21 @@ def Present(robot, coord, manip=None, render=True): This must be the right arm. @param render Render tsr samples during planning """ - focus_trans = numpy.eye(4) - focus_trans[0:3, 3] = coord - - present_tsr = robot.tsrlibrary(None, 'present', focus_trans, manip) - if manip is None: manip = robot.right_arm if manip.GetName() != 'right': raise prpy.exceptions.PrPyException('Presenting is only defined \ on the right arm.') + + focus_trans = numpy.eye(4, dtype='float') + focus_trans[0:3, 3] = coord + present_tsr = robot.tsrlibrary(None, 'present', focus_trans, manip) p = openravepy.KinBody.SaveParameters - with robot.CreateRobotStateSaver(p.ActiveManipulator): + with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): robot.SetActiveManipulator(manip) + robot.SetActiveDOF(manip.GetArmIndices) with prpy.viz.RenderTSRList(present_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(present_tsr, execute=True) manip.hand.MoveHand(f1=1, f2=1, f3=1, spread=3.14) @@ -160,7 +161,7 @@ def Sweep(robot, start_coords, end_coords, manip=None, margin=0.3, render=True): hand_pose = numpy.array([[ 0, -1, 0, start_coords[0]], [ 0, 0, 1, (start_coords[1]+ee_offset)], [-1, 0, 0, (start_coords[2]+margin)], - [ 0, 0, 0, 1]]) + [ 0, 0, 0, 1]], dtype='float') elif manip.GetName() == 'left': hand = robot.left_hand @@ -168,24 +169,29 @@ def Sweep(robot, start_coords, end_coords, manip=None, margin=0.3, render=True): hand_pose = numpy.array([[ 0, 1, 0, start_coords[0]], [ 0, 0, -1, (start_coords[1]+ee_offset)], [-1, 0, 0, (start_coords[2]+margin)], - [ 0, 0, 0, 1]]) + [ 0, 0, 0, 1]], dtype='float') else: raise prpy.exceptions.PrPyException('Manipulator does not have an \ associated hand') - end_trans = numpy.eye(4) + end_trans = numpy.eye(4, dtype='float') end_trans[0:3, 3] = end_coords hand.MoveHand(f1=1, f2=1, f3=1, spread=3.14) - manip.PlanToEndEffectorPose(hand_pose) + q = openravepy.KinBody.SaveParameters + with robot.CreateRobotStateSaver(q.ActiveManipulator | q.ActiveDOF): + robot.SetActiveManipulator(manip) + robot.SetActiveDOF(manip.GetArmIndices) + manip.PlanToEndEffectorPose(hand_pose, execute=True) #TSR to sweep to end position sweep_tsr = robot.tsrlibrary(None, 'sweep', end_trans, manip) p = openravepy.KinBody.SaveParameters - with robot.CreateRobotStateSaver(p.ActiveManipulator): + with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): robot.SetActiveManipulator(manip) + robot.SetActiveDOF(manip) with prpy.viz.RenderTSRList(sweep_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(sweep_tsr, execute=True) @@ -217,8 +223,12 @@ def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, release=True, render=T #'Unlift' the object, so place it back down unlift_tsr = robot.tsrlibrary(obj, 'lift', manip, distance=-distance) - with prpy.viz.RenderTSRList(unlift_tsr, robot.GetEnv(), render=render): - robot.PlanToTSR(unlift_tsr, execute=True) + p = openravepy.KinBody.SaveParameters + with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): + robot.SetActiveManipulator(manip) + robot.SetActiveDOF(manip) + with prpy.viz.RenderTSRList(unlift_tsr, robot.GetEnv(), render=render): + robot.PlanToTSR(unlift_tsr, execute=True) if release: with robot.GetEnv(): @@ -277,20 +287,20 @@ def HaltHand(robot, manip=None): if manip.GetName() == 'right': pose = numpy.array([5.03348748, -1.57569674, 1.68788069, 2.06769058, -1.66834313, - 1.53679821, 0.21175342]) + 1.53679821, 0.21175342], dtype='float') manip.PlanToConfiguration(pose, execute=True) robot.right_hand.MoveHand(f1=0, f2=0, f3=0, spread=3.14) elif manip.GetName() == 'left': pose = numpy.array([ 1.30614268, -1.76 , -1.57063853, 2.07228362, 1.23918377, - 1.46215605, -0.12918424]) + 1.46215605, -0.12918424], dtype='float') manip.PlanToConfiguration(pose, execute=True) robot.left_hand.MoveHand(f1=0, f2=0, f3=0, spread=3.14) else: - raise prpy.exceptions.PrPyException('Stop is only defined \ - for the left and right arm.') + raise prpy.exceptions.PrPyException( + 'Stop is only defined for the left and right arm.') @ActionMethod def MiddleFinger(robot, manip=None): @@ -305,7 +315,7 @@ def MiddleFinger(robot, manip=None): if manip.GetName() == 'right': right_dof = numpy.array([ 5.03348748, -1.57569674, 1.68788069, 2.06769058, -1.66834313, - 1.53679821, 0.21175342]) + 1.53679821, 0.21175342], dtype='float') manip.PlanToConfiguration(right_dof, execute=True) robot.right_hand.MoveHand(f1=2, f2=2, f3=0, spread=3.14) @@ -313,7 +323,7 @@ def MiddleFinger(robot, manip=None): elif manip.GetName() == 'left': left_dof = numpy.array([ 1.30614268, -1.76 , -1.57063853, 2.07228362, 1.23918377, - 1.46215605, -0.12918424]) + 1.46215605, -0.12918424], dtype='float') manip.PlanToConfiguration(left_dof, execute=True) robot.left_hand.MoveHand(f1=2, f2=2, f3=0, spread=3.14) @@ -334,19 +344,14 @@ def Wave(robot): env = robot.GetEnv() wave_path = FindCatkinResource('herbpy', 'config/waveTrajs/') - try: - traj0 = load_trajectory(env, join(wave_path, 'wave0.xml')) - traj1 = load_trajectory(env, join(wave_path, 'wave1.xml')) - traj2 = load_trajectory(env, join(wave_path, 'wave2.xml')) - traj3 = load_trajectory(env, join(wave_path, 'wave3.xml')) - except IOError as e: - raise IOError(str(e)) + traj0 = load_trajectory(env, join(wave_path, 'wave0.xml')) + traj1 = load_trajectory(env, join(wave_path, 'wave1.xml')) + traj2 = load_trajectory(env, join(wave_path, 'wave2.xml')) + traj3 = load_trajectory(env, join(wave_path, 'wave3.xml')) p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveManipulator): manip = robot.right_arm - robot.SetActiveManipulator(manip) - robot.HaltHand(manip=manip) robot.ExecuteTrajectory(traj0) robot.ExecuteTrajectory(traj1) From e67405577cd657bbc5780f8a9549e28528260c42 Mon Sep 17 00:00:00 2001 From: Rachel Date: Tue, 27 Oct 2015 15:51:51 -0400 Subject: [PATCH 09/12] rogue tests, import and string cleanup --- package.xml | 1 + src/herbpy/action/__init__.py | 2 +- src/herbpy/action/grasping.py | 2 +- src/herbpy/action/rogue.py | 70 +++++++++++++++++------------------ tests/rogue_tests.py | 68 ++++++++++++++++++++++++++++++++++ 5 files changed, 105 insertions(+), 38 deletions(-) create mode 100755 tests/rogue_tests.py diff --git a/package.xml b/package.xml index 0e99580..a3fba0a 100644 --- a/package.xml +++ b/package.xml @@ -33,4 +33,5 @@ or_ompl or_sbpl openrave + python-nose diff --git a/src/herbpy/action/__init__.py b/src/herbpy/action/__init__.py index 42ffe44..c7c6cf2 100755 --- a/src/herbpy/action/__init__.py +++ b/src/herbpy/action/__init__.py @@ -1,3 +1,3 @@ from grasping import PushGrasp, Grasp -from rogue import * +from rogue import PointAt, PresentAt, SweepAt, Point, Present, Sweep, Exhibit, NodYes, NodNo, HaltHand, MiddleFinger, Wave, GetPointFrom from blocks import GrabBlock diff --git a/src/herbpy/action/grasping.py b/src/herbpy/action/grasping.py index 2737354..24b79d0 100755 --- a/src/herbpy/action/grasping.py +++ b/src/herbpy/action/grasping.py @@ -77,7 +77,7 @@ def HerbGrasp(robot, obj, push_distance=None, manip=None, # Plan to the grasp with prpy.viz.RenderTSRList(tsrlist, robot.GetEnv(), render=render): - manip.PlanToTSR(tsrlist) + manip.PlanToTSR(tsrlist, execute=True) if push_distance is not None: ee_in_world = manip.GetEndEffectorTransform() diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index 2e6e5fa..ee73038 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -95,8 +95,8 @@ def Point(robot, coord, manip=None, render=False): manip = robot.right_arm if manip.GetName() != 'right': - raise prpy.exceptions.PrPyException('Pointing is only defined \ - on the right arm.') + raise prpy.exceptions.PrPyException('Pointing is only defined' + ' on the right arm.') focus_trans = numpy.eye(4, dtype='float') focus_trans[0:3, 3] = coord @@ -105,7 +105,7 @@ def Point(robot, coord, manip=None, render=False): p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): robot.SetActiveManipulator(manip) - robot.SetActiveDOF(manip.GetArmIndices) + robot.SetActiveDOFs(manip.GetArmIndices()) with prpy.viz.RenderTSRList(point_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(point_tsr, execute=True) manip.hand.MoveHand(f1=2.4, f2=0.8, f3=2.4, spread=3.14) @@ -123,8 +123,8 @@ def Present(robot, coord, manip=None, render=True): manip = robot.right_arm if manip.GetName() != 'right': - raise prpy.exceptions.PrPyException('Presenting is only defined \ - on the right arm.') + raise prpy.exceptions.PrPyException('Presenting is only defined' + ' on the right arm.') focus_trans = numpy.eye(4, dtype='float') focus_trans[0:3, 3] = coord @@ -133,7 +133,7 @@ def Present(robot, coord, manip=None, render=True): p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): robot.SetActiveManipulator(manip) - robot.SetActiveDOF(manip.GetArmIndices) + robot.SetActiveDOFs(manip.GetArmIndices()) with prpy.viz.RenderTSRList(present_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(present_tsr, execute=True) manip.hand.MoveHand(f1=1, f2=1, f3=1, spread=3.14) @@ -172,8 +172,8 @@ def Sweep(robot, start_coords, end_coords, manip=None, margin=0.3, render=True): [ 0, 0, 0, 1]], dtype='float') else: - raise prpy.exceptions.PrPyException('Manipulator does not have an \ - associated hand') + raise prpy.exceptions.PrPyException('Manipulator does not have an' + ' associated hand') end_trans = numpy.eye(4, dtype='float') end_trans[0:3, 3] = end_coords @@ -182,7 +182,7 @@ def Sweep(robot, start_coords, end_coords, manip=None, margin=0.3, render=True): q = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(q.ActiveManipulator | q.ActiveDOF): robot.SetActiveManipulator(manip) - robot.SetActiveDOF(manip.GetArmIndices) + robot.SetActiveDOFs(manip.GetArmIndices()) manip.PlanToEndEffectorPose(hand_pose, execute=True) #TSR to sweep to end position @@ -191,7 +191,7 @@ def Sweep(robot, start_coords, end_coords, manip=None, margin=0.3, render=True): p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): robot.SetActiveManipulator(manip) - robot.SetActiveDOF(manip) + robot.SetActiveDOFs(manip.GetArmIndices()) with prpy.viz.RenderTSRList(sweep_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(sweep_tsr, execute=True) @@ -205,28 +205,28 @@ def Exhibit(robot, obj, manip=None, distance=0.1, wait=2, release=True, render=T @param wait The amount of time the object will be held up in seconds @param render Render tsr samples during planning """ + with robot.GetEnv(): if manip is None: manip = robot.GetActiveManipulator() preconfig = manip.GetDOFValues() - robot.Grasp(obj) - - #Lift the object - write more tsrs - lift_tsr = robot.tsrlibrary(obj, 'lift', manip, distance=distance) - - with prpy.viz.RenderTSRList(lift_tsr, robot.GetEnv(), render=render): - robot.PlanToTSR(lift_tsr, execute=True) + robot.Grasp(obj) - #Wait for 'time' - time.sleep(wait) - - #'Unlift' the object, so place it back down - unlift_tsr = robot.tsrlibrary(obj, 'lift', manip, distance=-distance) - p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveManipulator | p.ActiveDOF): robot.SetActiveManipulator(manip) - robot.SetActiveDOF(manip) + robot.SetActiveDOFs(manip.GetArmIndices()) + + #Lift the object + lift_tsr = robot.tsrlibrary(obj, 'lift', manip, distance=distance) + with prpy.viz.RenderTSRList(lift_tsr, robot.GetEnv(), render=render): + robot.PlanToTSR(lift_tsr, execute=True) + + #Wait for 'time' + time.sleep(wait) + + #'Unlift' the object, so place it back down + unlift_tsr = robot.tsrlibrary(obj, 'lift', manip, distance=-distance) with prpy.viz.RenderTSRList(unlift_tsr, robot.GetEnv(), render=render): robot.PlanToTSR(unlift_tsr, execute=True) @@ -300,7 +300,7 @@ def HaltHand(robot, manip=None): robot.left_hand.MoveHand(f1=0, f2=0, f3=0, spread=3.14) else: raise prpy.exceptions.PrPyException( - 'Stop is only defined for the left and right arm.') + 'HaltHand is only defined for the left and right arm.') @ActionMethod def MiddleFinger(robot, manip=None): @@ -309,7 +309,7 @@ def MiddleFinger(robot, manip=None): @param manip The manipulator being used to give the middle finger. Must be either the right or left arm. """ - if manip == None: + if manip is None: manip = robot.GetActiveManipulator() if manip.GetName() == 'right': @@ -328,8 +328,8 @@ def MiddleFinger(robot, manip=None): manip.PlanToConfiguration(left_dof, execute=True) robot.left_hand.MoveHand(f1=2, f2=2, f3=0, spread=3.14) else: - raise prpy.exceptions.PrPyException('The middle finger is only defined \ - for the left and right arm.') + raise prpy.exceptions.PrPyException('The middle finger is only defined' + ' for the left and right arm.') @ActionMethod @@ -349,11 +349,9 @@ def Wave(robot): traj2 = load_trajectory(env, join(wave_path, 'wave2.xml')) traj3 = load_trajectory(env, join(wave_path, 'wave3.xml')) - p = openravepy.KinBody.SaveParameters - with robot.CreateRobotStateSaver(p.ActiveManipulator): - manip = robot.right_arm - robot.HaltHand(manip=manip) - robot.ExecuteTrajectory(traj0) - robot.ExecuteTrajectory(traj1) - robot.ExecuteTrajectory(traj2) - robot.ExecuteTrajectory(traj3) + manip = robot.right_arm + robot.HaltHand(manip=manip) + robot.ExecuteTrajectory(traj0) + robot.ExecuteTrajectory(traj1) + robot.ExecuteTrajectory(traj2) + robot.ExecuteTrajectory(traj3) diff --git a/tests/rogue_tests.py b/tests/rogue_tests.py new file mode 100755 index 0000000..32a7cf4 --- /dev/null +++ b/tests/rogue_tests.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python +PKG = 'herbpy' +import roslib; roslib.load_manifest(PKG) +import numpy, unittest +import herbpy, prpy + +env, robot = herbpy.initialize(sim=True) +fuze = prpy.rave.add_object(env, 'fuze_bottle', 'objects/fuze_bottle.kinbody.xml') + +class RogueTest(unittest.TestCase): + def setUp(self): + self.env, self.robot, self.fuze = env, robot, fuze + self.fuze_pose = numpy.eye(4) + self.fuze_pose[0:3, 3] = [0.8, -0.3, 0.4] + self.fuze.SetTransform(self.fuze_pose) + self.fuze.Enable(True) + + def test_Pointing(self): + self.robot.PointAt([1, 1, 1]) + herbpy.action.Point(robot, [1, 1, 1]) + + def test_Presenting(self): + self.robot.PresentAt([0.5, 0, 1]) + herbpy.action.Present(robot, [0.5, 0, 1]) + + def test_Sweeping(self): + self.robot.SweepAt([0.5, 0, 0.5], [0.7, 0, 0.5]) + herbpy.action.Sweep(robot, [0.7, 0, 0.5], [0.5, 0, 0.5]) + + def test_GetPointFrom(self): + kinbody_coord = herbpy.action.GetPointFrom(self.env, self.fuze) + numpy.testing.assert_almost_equal(kinbody_coord, self.fuze_pose[0:3, 3]) + + test_array = numpy.array([0, 1, 2]) + numpyarray_coord = herbpy.action.GetPointFrom(self.env, test_array) + numpy.testing.assert_almost_equal(numpyarray_coord, test_array) + + test_list = [0, 1, 2] + list_coord = herbpy.action.GetPointFrom(self.env, test_list) + numpy.testing.assert_almost_equal(list_coord, test_list) + + test_tuple = (0, 1, 2) + tuple_coord = herbpy.action.GetPointFrom(self.env, test_tuple) + numpy.testing.assert_almost_equal(tuple_coord, test_tuple) + + transform_coord = herbpy.action.GetPointFrom(self.env, self.fuze_pose) + numpy.testing.assert_almost_equal(transform_coord, self.fuze_pose[0:3, 3]) + + def test_Exhibiting(self): + self.robot.Exhibit(self.fuze) + + def test_Nodding(self): + robot.NodYes() + robot.NodNo() + + def test_HaltHand(self): + robot.HaltHand() + + def test_MiddleFinger(self): + robot.MiddleFinger() + + def test_Wave(self): + robot.Wave() + + +if __name__ == '__main__': + import rosunit + rosunit.unitrun(PKG, 'test_rogue', RogueTest) From cf4a6add37a1444f0cd035c4f06b9478160a9017 Mon Sep 17 00:00:00 2001 From: Rachel Date: Wed, 28 Oct 2015 10:22:19 -0400 Subject: [PATCH 10/12] convert to using util function for get manip idx --- src/herbpy/tsr/generic.py | 28 ++++------------------------ tests/rogue_tests.py | 1 + 2 files changed, 5 insertions(+), 24 deletions(-) diff --git a/src/herbpy/tsr/generic.py b/src/herbpy/tsr/generic.py index 15fa308..86da2fa 100644 --- a/src/herbpy/tsr/generic.py +++ b/src/herbpy/tsr/generic.py @@ -1,6 +1,7 @@ import numpy from prpy.tsr.tsrlibrary import TSRFactory from prpy.tsr.tsr import TSR, TSRChain +from prpy.util import GetManipulatorIndex @TSRFactory('herb', None, 'point') def point_obj(robot, transform, manip=None): @@ -10,15 +11,8 @@ def point_obj(robot, transform, manip=None): @param manip The manipulator to point with. This must be the right arm. """ - with robot.GetEnv(): - if manip is None: - manip = robot.right_arm + (manip, manip_idx) = GetManipulatorIndex(robot, manip) - with robot.CreateRobotStateSaver( - robot.SaveParameters.ActiveManipulator): - robot.SetActiveManipulator(manip) - manip_idx = manip.GetRobot().GetActiveManipulatorIndex() - if manip.GetName() != 'right': raise prpy.exceptions.PrPyException('Pointing is only defined on the right arm.') @@ -59,14 +53,7 @@ def present_obj(robot, transform, manip=None): @param manip The manipulator to present. This must be the right arm. """ - with robot.GetEnv(): - if manip is None: - manip = robot.right_arm - - with robot.CreateRobotStateSaver( - robot.SaveParameters.ActiveManipulator): - robot.SetActiveManipulator(manip) - manip_idx = manip.GetRobot().GetActiveManipulatorIndex() + (manip, manip_idx) = GetManipulatorIndex(robot, manip) if manip.GetName() != 'right': raise prpy.exceptions.PrpyException('Presenting is only defined for the right arm.') @@ -101,14 +88,7 @@ def sweep_objs(robot, transform, manip=None): @param manip The manipulator to sweep. """ - with robot.GetEnv(): - if manip is None: - manip = robot.right_arm - - with robot.CreateRobotStateSaver( - robot.SaveParameters.ActiveManipulator): - robot.SetActiveManipulator(manip) - manip_idx = manip.GetRobot().GetActiveManipulatorIndex() + (manip, manip_idx) = GetManipulatorIndex(robot, manip) #TSR for the goal ee_offset = 0.2 diff --git a/tests/rogue_tests.py b/tests/rogue_tests.py index 32a7cf4..1bd9cf1 100755 --- a/tests/rogue_tests.py +++ b/tests/rogue_tests.py @@ -5,6 +5,7 @@ import herbpy, prpy env, robot = herbpy.initialize(sim=True) +robot.right_arm.SetActive() fuze = prpy.rave.add_object(env, 'fuze_bottle', 'objects/fuze_bottle.kinbody.xml') class RogueTest(unittest.TestCase): From 96f2a0e23829400e0a8b7efa8dff20e02f3cdce4 Mon Sep 17 00:00:00 2001 From: Rachel Date: Wed, 28 Oct 2015 12:38:18 -0400 Subject: [PATCH 11/12] in gestures, if no manip set active manip --- src/herbpy/action/rogue.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/herbpy/action/rogue.py b/src/herbpy/action/rogue.py index ee73038..42efee0 100644 --- a/src/herbpy/action/rogue.py +++ b/src/herbpy/action/rogue.py @@ -92,7 +92,7 @@ def Point(robot, coord, manip=None, render=False): @param render Render tsr samples during planning """ if manip is None: - manip = robot.right_arm + manip = robot.GetActiveManipulator() if manip.GetName() != 'right': raise prpy.exceptions.PrPyException('Pointing is only defined' @@ -120,7 +120,7 @@ def Present(robot, coord, manip=None, render=True): @param render Render tsr samples during planning """ if manip is None: - manip = robot.right_arm + manip = robot.GetActiveManipulator() if manip.GetName() != 'right': raise prpy.exceptions.PrPyException('Presenting is only defined' From 0d1b0ae074154461c26c2f842f025a483d88f47b Mon Sep 17 00:00:00 2001 From: Rachel Date: Wed, 28 Oct 2015 13:15:49 -0400 Subject: [PATCH 12/12] adhering to testing convention --- tests/rogue_tests.py | 33 ++++++++++++++++----------------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/tests/rogue_tests.py b/tests/rogue_tests.py index 1bd9cf1..2293c1b 100755 --- a/tests/rogue_tests.py +++ b/tests/rogue_tests.py @@ -1,32 +1,32 @@ #!/usr/bin/env python -PKG = 'herbpy' -import roslib; roslib.load_manifest(PKG) +import roslib import numpy, unittest import herbpy, prpy -env, robot = herbpy.initialize(sim=True) -robot.right_arm.SetActive() -fuze = prpy.rave.add_object(env, 'fuze_bottle', 'objects/fuze_bottle.kinbody.xml') - class RogueTest(unittest.TestCase): def setUp(self): - self.env, self.robot, self.fuze = env, robot, fuze + self.env, self.robot = herbpy.initialize(sim=True) + self.fuze = prpy.rave.add_object(self.env, 'fuze_bottle', 'objects/fuze_bottle.kinbody.xml') + self.robot.right_arm.SetActive() self.fuze_pose = numpy.eye(4) self.fuze_pose[0:3, 3] = [0.8, -0.3, 0.4] self.fuze.SetTransform(self.fuze_pose) self.fuze.Enable(True) + def tearDown(self): + self.env.Destroy() + def test_Pointing(self): self.robot.PointAt([1, 1, 1]) - herbpy.action.Point(robot, [1, 1, 1]) - + herbpy.action.Point(self.robot, [1, 1, 1]) + def test_Presenting(self): self.robot.PresentAt([0.5, 0, 1]) - herbpy.action.Present(robot, [0.5, 0, 1]) + herbpy.action.Present(self.robot, [0.5, 0, 1]) def test_Sweeping(self): self.robot.SweepAt([0.5, 0, 0.5], [0.7, 0, 0.5]) - herbpy.action.Sweep(robot, [0.7, 0, 0.5], [0.5, 0, 0.5]) + herbpy.action.Sweep(self.robot, [0.7, 0, 0.5], [0.5, 0, 0.5]) def test_GetPointFrom(self): kinbody_coord = herbpy.action.GetPointFrom(self.env, self.fuze) @@ -51,18 +51,17 @@ def test_Exhibiting(self): self.robot.Exhibit(self.fuze) def test_Nodding(self): - robot.NodYes() - robot.NodNo() + self.robot.NodYes() + self.robot.NodNo() def test_HaltHand(self): - robot.HaltHand() + self.robot.HaltHand() def test_MiddleFinger(self): - robot.MiddleFinger() + self.robot.MiddleFinger() def test_Wave(self): - robot.Wave() - + self.robot.Wave() if __name__ == '__main__': import rosunit