def stop(self):
if self._status == 'ok':
robot.stopMotion()
self._status = ''
robot.enableCollisionChecking(False)
return None
def start(self, args):
print "Calling JointPose start", args
if 'part' not in args and 'parts' not in args:
self.onInvalidParameters("invalid argument list")
return False
if 'position' not in args and 'positions' not in args:
self.onInvalidParameters("invalid argument list")
return False
if 'part' in args:
parts = [args['part']]
else:
parts = args['parts']
if 'position' in args:
angles = [args['position']]
else:
angles = args['positions']
if len(parts) != len(angles):
self.onInvalidParameters(
"Must have same number of parts as positions")
speed = args.get('speed', 1.0)
if 'safe' in args:
print "Safety flag:", args['safe']
safe = args.get('safe', False)
if safe:
robot.enableCollisionChecking(True)
print "Collision checking enabled on JointPose task"
else:
robot.enableCollisionChecking(False)
print "Collision checking disabled on JointPose task"
self.doneTests = []
for part, angle in zip(parts, angles):
if part == 'left' or part == 0:
if len(angle) != 7:
self.onInvalidParameters(
"invalid 'position' argument for left arm, not size 7")
return False
robot.left_mq.setRamp(angle, speed)
self.doneTests.append(lambda: not robot.left_mq.moving())
elif part == 'right' or part == 1:
if len(angle) != 7:
self.onInvalidParameters(
"invalid 'position' argument for right arm, not size 7"
)
return False
robot.right_mq.setRamp(angle)
self.doneTests.append(lambda: not robot.right_mq.moving())
elif part == 'base' or part == 2:
if len(angle) != 3:
self.onInvalidParameters(
"invalid 'position' argument for base, not size 3")
return False
robot.base.moveOdometryPosition(angle[0], angle[1], angle[2])
self.doneTests.append(lambda: not robot.base.moving())
else:
self.onInvalidParameters("invalid 'part' setting: %s" %
(str(part), ))
return False
if safe:
robot.enableCollisionChecking(False)
self._status = 'ok'
return True
def start(self,args):
print "Calling CartesianPose start",args
if 'limb' not in args:
self.onInvalidParameters("invalid argument list")
return False
if 'position' not in args:
self.onInvalidParameters("invalid argument list")
return False
limb = args['limb']
position = args['position']
rotation = args.get('rotation',[])
toolCenterPoint = args.get('toolCenterPoint',None)
maxJointDeviation = args.get('maxJointDeviation',0)
speed = args.get('speed',1.0)
safe = args.get('safe',False)
if safe:
print 'safe'
robot.enableCollisionChecking(True)
robot.left_limb.enableSelfCollisionAvoidance(False)
robot.right_limb.enableSelfCollisionAvoidance(False)
else:
robot.enableCollisionChecking(False)
robot.left_limb.enableSelfCollisionAvoidance(True)
robot.right_limb.enableSelfCollisionAvoidance(True)
if limb == 'left' or limb == 0:
if len(position) !=3:
self.onInvalidParameters("invalid 'position' argument, not size 3")
return False
if len(rotation) > 0 and len(rotation)!=9:
self.onInvalidParameters("invalid 'rotation' argument, not size 3")
return False
if len(rotation) == 0:
rotation = robot.left_ee.commandedTransform()[0]
if toolCenterPoint is not None:
robot.left_ee.setOffset(toolCenterPoint)
robot.left_ee.moveTo(rotation, position, maxJointDeviation=maxJointDeviation)
self.mqs = [robot.left_mq]
elif limb == 'right' or limb == 1:
if len(position) !=3:
self.onInvalidParameters("invalid 'position' argument, not size 3")
return False
if len(rotation) > 0 and len(rotation)!=9:
self.onInvalidParameters("invalid 'rotation' argument, not size 9")
return False
if len(rotation) == 0:
rotation = robot.right_ee.commandedTransform()[0]
if toolCenterPoint is not None:
robot.right_ee.setOffset(toolCenterPoint)
robot.right_ee.moveTo(rotation, position, maxJointDeviation=maxJointDeviation)
self.mqs = [robot.right_mq]
elif limb == 'both' or limb == 2:
if len(position) != 6:
self.onInvalidParameters("invalid 'position' argument, not size 6")
return False
if len(rotation) > 0 and len(rotation)!=18:
self.onInvalidParameters("invalid 'rotation' argument, not size 18")
return False
if len(rotation) == 0:
rotation = robot.left_ee.commandedTransform()[0] + robot.right_ee.commandedTransform()[0]
if toolCenterPoint is not None:
robot.right_ee.setOffset(toolCenterPoint)
robot.left_ee.setOffset(toolCenterPoint)
resl = robot.left_ee.moveTo(rotation[0:9], position[0:3], maxJointDeviation=maxJointDeviation)
resr = robot.right_ee.moveTo(rotation[9:18], position[3:6], maxJointDeviation=maxJointDeviation)
print "Left result",resl
print "Right result",resr
self.mqs = [robot.left_mq,robot.right_mq]
else:
self.onInvalidParameters("invalid 'limb' setting: %s"%(limb,))
return False
if safe:
robot.enableCollisionChecking(False)
self._status = 'ok'
return True
def start(self, args):
# Error checking the input arguments from the dictionary
print "Calling PhilipPoseVel start", args
"""if 'limb' not in args:
self.onInvalidParameters("invalid argument list")
return False"""
if 'linear' not in args and 'angular' not in args:
self.onInvalidParameters("invalid argument list")
return False
if 'angular' not in args:
self.onInvalidParameters(
"invalid argument list, must have velocity specified")
return False
if 'position' not in args:
self.onInvalidParameters(
"invalid argument list, must have joint position specified")
return False
if len(args['linear']) != 3:
self.onInvalidParameters(
"invalid argument list, velocity command is not length 3")
return False
if len(args['position']) != 7:
self.onInvalidParameters(
"invalid argument list, joint position command is not length 7"
)
return False
# Now I extract the arguments from the pose/vel command
#--- Safe Control ---
safe = args.get('safe', False)
if safe:
print 'safe'
robot.enableCollisionChecking(True)
robot.left_limb.enableSelfCollisionAvoidance(False)
#robot.right_limb.enableSelfCollisionAvoidance(False)
else:
robot.enableCollisionChecking(False)
robot.left_limb.enableSelfCollisionAvoidance(True)
#robot.right_limb.enableSelfCollisionAvoidance(True)
#--- End Safe Control ---
# We do not actually care about which limb -- the left is pose control and the right one is cartesian control
#limb = args['limb']
# velocity
dlinear = args['linear']
dangular = args['angular']
toolCenterPoint = args.get('toolCenterPoint', None)
# position
angles = args['position']
# Time
maxTime = args.get('maxTime', 1.0)
if maxTime < 0:
self.onInvalidParameters("maxTime argument is negative")
return False
# Now I set the Zero configuration for velocity control
if len(dlinear) > 0 and len(dlinear) != 3:
self.onInvalidParameters(
"invalid 'linear' argument for linear velocity, not size 3")
return False
if len(dangular) > 0 and len(dangular) != 3:
self.onInvalidParameters(
"invalid 'angular' argument for angular velocity, not size 3")
return False
if len(dlinear) == 3 and len(dangular) == 0:
dangular = [0.0, 0.0, 0.0]
if len(dangular) == 3 and len(dlinear) == 0:
dlinear = [0.0, 0.0, 0.0]
#deadband
for i, v in enumerate(dlinear):
if abs(v) < 1e-4: dlinear[i] = 0
for i, v in enumerate(dangular):
if abs(v) < 1e-4: dangular[i] = 0
if safe:
robot.enableCollisionChecking(True)
else:
robot.enableCollisionChecking(False)
# Command the left arm to a cartesian velocity
#robot.left_ee.moveTo(rotation, position, maxJointDeviation=maxJointDeviation)
robot.left_ee.driveCommand(dangular, dlinear)
# Command the right arm to a joint pose
#robot.right_ee.driveCommand(dangular, dlinear)
robot.right_mq.setRamp(angles)
self.mqs = [robot.left_mq, robot.right_mq]
self.endTime = time.time() + maxTime
if max(abs(v) for v in dangular + dlinear) == 0:
self._status = 'done'
else:
self._status = 'ok'
return True
def start(self, args):
if 'limb' not in args:
self.onInvalidParameters("invalid argument list")
return False
if 'linear' not in args and 'angular' not in args:
self.onInvalidParameters("invalid argument list")
return False
limb = args['limb']
dlinear = args['linear']
dangular = args['angular']
toolCenterPoint = args.get('toolCenterPoint', None)
maxTime = args.get('maxTime', 1.0)
safe = args.get('safe', False)
#deadband
for i, v in enumerate(dlinear):
if abs(v) < 1e-4: dlinear[i] = 0
for i, v in enumerate(dangular):
if abs(v) < 1e-4: dangular[i] = 0
if safe:
robot.enableCollisionChecking(True)
else:
robot.enableCollisionChecking(False)
if limb == 'left' or limb == 0:
if len(dlinear) > 0 and len(dlinear) != 3:
self.onInvalidParameters(
"invalid 'linear' argument for linear velocity, not size 3"
)
return False
if len(dangular) > 0 and len(dangular) != 3:
self.onInvalidParameters(
"invalid 'angular' argument for angular velocity, not size 3"
)
return False
if len(dlinear) == 3 and len(dangular) == 0:
dangular = [0.0, 0.0, 0.0]
if len(dangular) == 3 and len(dlinear) == 0:
dlinear = [0.0, 0.0, 0.0]
if toolCenterPoint is not None:
robot.left_ee.setOffset(toolCenterPoint)
robot.left_ee.driveCommand(dangular, dlinear)
elif limb == 'right' or limb == 1:
if len(dlinear) > 0 and len(dlinear) != 3:
self.onInvalidParameters(
"invalid 'linear' argument for linear velocity, not size 3"
)
return False
if len(dangular) > 0 and len(dangular) != 3:
self.onInvalidParameters(
"invalid 'angular' argument for angular velocity, not size 3"
)
return False
if len(dlinear) == 3 and len(dangular) == 0:
dangular = [0.0, 0.0, 0.0]
if len(dangular) == 3 and len(dlinear) == 0:
dlinear = [0.0, 0.0, 0.0]
if toolCenterPoint is not None:
robot.right_ee.setOffset(toolCenterPoint)
robot.right_ee.driveCommand(dangular, dlinear)
elif limb == 'both' or limb == 2:
if len(dlinear) > 0 and len(dlinear) != 6:
self.onInvalidParameters(
"invalid 'linear' argument for linear velocity, not size 6"
)
return False
if len(dangular) > 0 and len(dangular) != 6:
self.onInvalidParameters(
"invalid 'angular' argument for angular velocity, not size 6"
)
return False
if len(dlinear) == 6 and len(dangular) == 0:
dangular = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
if len(dangular) == 6 and len(dlinear) == 0:
dlinear = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
if toolCenterPoint is not None:
robot.right_ee.setOffset(toolCenterPoint)
robot.left_ee.setOffset(toolCenterPoint)
robot.right_ee.driveCommand(dangular[0:3], dlinear[0:3])
robot.left_ee.driveCommand(dangular[3:6], dlinear[3:6])
else:
self.onInvalidParameters("invalid 'limb' setting: %s" % (limb, ))
return False
if max(abs(v) for v in dangular + dlinear) == 0:
self._status = 'done'
else:
self._status = 'ok'
return True
def stop(self):
self._status = ''
robot.enableCollisionChecking(False)
return None