def __init__(self, d_robot, controller, arm, verbose=False):
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Variables...! #
if arm == 'l': self.arm = 'left'
else: self.arm = 'right'
self.stop_motion = False
self.verbose = verbose
self.bowl_frame_kinect = None
self.mouth_frame_kinect = None
self.default_frame = PyKDL.Frame()
self.initCommsForArmReach()
self.initParamsForArmReach()
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
if verbose:
print "--------------------------------"
print "Current "+self.arm+" arm joint angles"
print self.getJointAngles()
print "Current "+self.arm+" arm pose"
print self.getEndeffectorPose(tool=0)
print "Current "+self.arm+" arm orientation (w/ euler rpy)"
print self.getEndeffectorRPY(tool=0) #*180.0/np.pi
print "--------------------------------"
break
rate.sleep()
rospy.loginfo("Arm Reach Action is initialized.")
def __init__(self, d_robot, controller, arm, verbose=False):
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Variables...! #
if arm == 'l': self.arm = 'left'
else: self.arm = 'right'
self.stop_motion = False
self.verbose = verbose
self.bowl_frame_kinect = None
self.mouth_frame_kinect = None
self.default_frame = PyKDL.Frame()
self.initCommsForArmReach()
self.initParamsForArmReach()
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
if verbose:
print "--------------------------------"
print "Current " + self.arm + " arm joint angles"
print self.getJointAngles()
print "Current " + self.arm + " arm pose"
print self.getEndeffectorPose(tool=0)
print "Current " + self.arm + " arm orientation (w/ euler rpy)"
print self.getEndeffectorRPY(tool=0) #*180.0/np.pi
print "--------------------------------"
break
rate.sleep()
rospy.loginfo("Arm Reach Action is initialized.")
def __init__(self, d_robot, controller, arm='l'):
mpcBaseAction.__init__(self, d_robot, controller, arm)
# service request
self.reach_service = rospy.Service('/door_opening/arm_reach_enable',
None_Bool, self.start_cb)
def __init__(self, d_robot, controller, arm): #removed arm= 'l' so I can use right arm as well as an option
mpcBaseAction.__init__(self, d_robot, controller, arm)
rospy.Subscriber('hrl_feeding_task/RYDS_CupLocation', PoseStamped, self.bowlPoseKinectCallback)
self.reach_service = rospy.Service('/arm_reach_enable', None_Bool, self.start_cb)
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
print "----------------------"
print "Current joint angles"
print self.getJointAngles()
print "Current pose"
print self.getEndeffectorPose()
print "----------------------"
break
if arm == 'r':
self.initialJointAnglesSideOfBody = [-1.570, 0, 0, -1.570, 3.141, 0, -1.570]
self.initialJointAnglesSideFacingFoward = [-1.570, 0, 0, -1.570, 1.570, -1.570, -1.570]
else:
self.initialJointAnglesSideOfBody = [1.570, 0, 0, -1.570, 3.141, 0, -4.712]
self.initialJointAnglesSideFacingFoward = [1.570, 0, 0, -1.570, 1.570, -1.570, -4.712]
self.pos = Point()
self.quat = Quaternion()
self.quatOrEulerSet = False
def __init__(
self, d_robot, controller, arm
): #removed arm= 'l' so I can use right arm as well as an option
mpcBaseAction.__init__(self, d_robot, controller, arm)
self.reach_service = rospy.Service('/arm_reach_enable', None_Bool,
self.start_cb)
handlePos = np.array[[0, 0, 0]]
framePos = np.array[[0, 0, 0]]
actionPositions = np.array[[0, 0, 0]]
actionEulers = np.array[[90, 0, 0]]
actionQuats = euler2quatArray(actionEulers)
def __init__(self, d_robot, controller, arm): #removed arm= 'l' so I can use right arm as well as an option
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Allows another node (client) to request specific actions corresponding to the right arm mpcBaseAction instance...
try:
self.setOrientRightGoalService = rospy.Service('/setOrientGoalRightService', PosQuatTimeoutSrv, self.setOrientGoalRight)
self.setStopRightService = rospy.Service('/setStopRightService', None_Bool, self.setStopRight)
self.setPostureGoalRightService = rospy.Service('/setPostureGoalRightService', AnglesTimeoutSrv, self.setPostureGoalRight)
print "Right arm services started!"
except:
print "Right arm services NOT started!"
def __init__(self, d_robot, controller, arm='l'):
mpcBaseAction.__init__(self, d_robot, controller, arm)
# service request
self.reach_service = rospy.Service('/adl/arm_reach_enable', None_Bool, self.start_cb)
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
print "----------------------"
print "Current joint angles"
print self.getJointAngles()
print "Current pose"
print self.getEndeffectorPose()
print "----------------------"
break
def __init__(self, d_robot, controller, arm='l'):
mpcBaseAction.__init__(self, d_robot, controller, arm)
# service request
self.reach_service = rospy.Service('/adl/arm_reach_enable', None_Bool,
self.start_cb)
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
print "----------------------"
print "Current joint angles"
print self.getJointAngles()
print "Current pose"
print self.getEndeffectorPose()
print "----------------------"
break
def __init__(
self, d_robot, controller, arm
): #removed arm= 'l' so I can use right arm as well as an option
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Allows another node (client) to request specific actions corresponding to the right arm mpcBaseAction instance...
try:
self.setOrientRightGoalService = rospy.Service(
'/setOrientGoalRightService', PosQuatTimeoutSrv,
self.setOrientGoalRight)
self.setStopRightService = rospy.Service('/setStopRightService',
None_Bool,
self.setStopRight)
self.setPostureGoalRightService = rospy.Service(
'/setPostureGoalRightService', AnglesTimeoutSrv,
self.setPostureGoalRight)
print "Right arm services started!"
except:
print "Right arm services NOT started!"
def __init__(self, d_robot, controller, arm, head_noise=False):
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Variables...! #
self.interrupted = False
self.iteration = 0 ##?????????????????????????????
self.posL = Point()
self.quatL = Quaternion()
self.posR = Point()
self.quatR = Quaternion()
#Declares bowl positions options ## looks redundant variables
self.bowl_pos_manual = None
self.bowl_pos_kinect = None
self.mouth_pos_manual = None
self.mouth_pos_kinect = None
# Artificial error
self.head_noise = head_noise
self.initCommsForArmReach()
self.initParamsForArmReach()
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
print "--------------------------------"
print "Current left arm joint angles"
print self.getJointAngles()
print "Current left arm pose"
print self.getEndeffectorPose()
print "--------------------------------"
break
rate.sleep()
rospy.loginfo("Arm Reach Action is initialized.")
def __init__(self, d_robot, controller, arm, head_noise=False):
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Variables...! #
self.interrupted = False
self.iteration = 0 ##?????????????????????????????
self.posL = Point()
self.quatL = Quaternion()
self.posR = Point()
self.quatR = Quaternion()
#Declares bowl positions options ## looks redundant variables
self.bowl_pos_manual = None
self.bowl_pos_kinect = None
self.mouth_pos_manual = None
self.mouth_pos_kinect = None
# Artificial error
self.head_noise = head_noise
self.initCommsForArmReach()
self.initParamsForArmReach()
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
print "--------------------------------"
print "Current left arm joint angles"
print self.getJointAngles()
print "Current left arm pose"
print self.getEndeffectorPose()
print "--------------------------------"
break
rate.sleep()
rospy.loginfo("Arm Reach Action is initialized.")
def __init__(self, d_robot, controller, arm): #removed arm= 'l' so I can use right arm as well as an option
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Subscribers to publishers of bowl location data
rospy.Subscriber('hrl_feeding_task/manual_bowl_location', PoseStamped, self.bowlPoseCallback) # hrl_feeding/bowl_location
rospy.Subscriber('hrl_feeding_task/RYDS_CupLocation', PoseStamped, self.bowlPoseKinectCallback)
rospy.Subscriber('hrl_feeding_task/emergency_arm_stop', String, self.stopCallback)
# service request
self.reach_service = rospy.Service('/arm_reach_enable', None_Bool, self.start_cb)
try:
self.setOrientGoalRight = rospy.ServiceProxy('/setOrientGoalRightService', PosQuatTimeoutSrv)
self.setStopRight = rospy.ServiceProxy('/setStopRightService', None_Bool)
self.setPostureGoalRight = rospy.ServiceProxy('/setPostureGoalRightService', AnglesTimeoutSrv)
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "CONNECTED TO RIGHT ARM SERVER YAY!"
except:
print "Oops, can't connect to right arm server!"
#Stored initialization joint angles
self.initialJointAnglesFrontOfBody = [0, 0.786, 0, -2, -3.141, 0, 0]
self.initialJointAnglesSideOfBodyLEFT = [1.570, 0, 0, -1.570, 3.141, 0, -1.570]
#self.initialJointAnglesSideOfBodyLEFT = [-1.570, 0, 0, -1.570, 3.141, 0, -1.570]
self.initialJointAnglesSideFacingFowardLEFT = [1.57, 0, 1.57, -1.57, 1.57, 0, -1.57]
self.initialJointAnglesSideOfBodyRIGHT = [-1.570, 0, 0, -1.570, 3.141, 0, -4.712]
self.initialJointAnglesSideFacingFowardRIGHT = [1.570, 0, 0, -1.570, 1.570, -1.570, -4.712]
#Variables...! #
armReachAction.iteration = 0
self.bowlPosOffsets = np.array([[-.01, 0, .4], #offsets from bowl_pos to left arm spoon
[-.01, 0, .008], #scooping motion is set of offsets
[.05, 0, .008],
[.05, 0, .008],
[.02, 0, .6],
[0, 0, 0],
[.02, 0, .6]])
self.bowlEulers = np.array([[90, -60, 0], #Euler angles, XYZ rotations for left arm spoon
[90, -60, 0], #controls scooping motion
[90, -60, 0],
[90, -30, 0],
[90, 0, 0],
[0, 0, 0],
[90, 0, 0]])
self.headPosOffsets = np.array([[.01, .085, -.01], #offsets from head_frame to left arm spoon
[.01, .2, .1], #feeding motion set of offsets
[0, 0, 0]])
self.headEulers = np.array([[90, 0, -90], #Euler angles, XYZ rotations for left arm spoon
[90, 0, -45], #controls feeding to mouth motion
[90, 0, 0]])
self.stopPos = np.array([[.7, .7, .5]])
self.stopEulers = np.array([[90, 0, 30]])
self.rightArmPosOffsets = np.array([[.02, .05, -.01],
[.02, .1, .3],
[0, 0, 0]]) #Set of pos offsets for the right arm end effector
self.rightArmEulers = np.array([[90, 0, 0],
[90, 0, 0],
[0, 0, 0]]) #Set of end effector angles for right arm
self.kinectBowlFoundPosOffsets = [-.08, -.04, 0]
self.timeouts = [20, 7, 4, 4, 4, 10, 7]
self.timeoutsR = [10, 10, 10]
self.kinectReachTimeout = 15
self.bowlQuatOffsets = self.euler2quatArray(self.bowlEulers) #converts the array of eulers to an array of quats
self.headQuatOffsets = self.euler2quatArray(self.headEulers)
self.rightArmQuatOffsets = self.euler2quatArray(self.rightArmEulers)
self.stopQuatOffsets = self.euler2quatArray(self.stopEulers)
print "Calculated quaternions:"
print self.bowlQuatOffsets
try:
print "--------------------------------"
raw_input("Register bowl & head position! Then press Enter \m")
#self.tf_lstnr.waitForTransform('/torso_lift_link', 'head_frame', rospy.Time.now(), rospy.Duration(10))
(self.headPos, self.headQuat) = self.tf_lstnr.lookupTransform('/torso_lift_link', 'head_frame', rospy.Time(0))
print "Recived head position: \n"
print self.headPos
print self.headQuat
print "--------------------------------"
raw_input("Press Enter to confirm.")
print "--------------------------------"
except(tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "Oops, can't get head_frame tf info!, trying again :)"
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
print "--------------------------------"
print "Current joint angles"
print self.getJointAngles()
print "Current pose"
print self.getEndeffectorPose()
print "--------------------------------"
break
rospy.spin()
def __init__(self, d_robot, controller, arm='l'):
mpcBaseAction.__init__(self, d_robot, controller, arm)
# service request
self.reach_service = rospy.Service('/door_opening/arm_reach_enable', None_Bool, self.start_cb)
def __init__(self, d_robot, controller, arm='l'):
mpcBaseAction.__init__(self, d_robot, controller, arm)
# service for ari's request
self.reach_service = rospy.Service('/base_selection/arm_reach_enable', None_Bool, self.start_cb)
def __init__(
self, d_robot, controller, arm
): #removed arm= 'l' so I can use right arm as well as an option
mpcBaseAction.__init__(self, d_robot, controller, arm)
#Subscribers to publishers of bowl location data
rospy.Subscriber('hrl_feeding_task/manual_bowl_location', PoseStamped,
self.bowlPoseCallback) # hrl_feeding/bowl_location
rospy.Subscriber('hrl_feeding_task/RYDS_CupLocation', PoseStamped,
self.bowlPoseKinectCallback)
rospy.Subscriber('hrl_feeding_task/emergency_arm_stop', String,
self.stopCallback)
# service request
self.reach_service = rospy.Service('/arm_reach_enable', None_Bool,
self.start_cb)
try:
self.setOrientGoalRight = rospy.ServiceProxy(
'/setOrientGoalRightService', PosQuatTimeoutSrv)
self.setStopRight = rospy.ServiceProxy('/setStopRightService',
None_Bool)
self.setPostureGoalRight = rospy.ServiceProxy(
'/setPostureGoalRightService', AnglesTimeoutSrv)
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "CONNECTED TO RIGHT ARM SERVER YAY!"
except:
print "Oops, can't connect to right arm server!"
#Stored initialization joint angles
self.initialJointAnglesFrontOfBody = [0, 0.786, 0, -2, -3.141, 0, 0]
self.initialJointAnglesSideOfBodyLEFT = [
1.570, 0, 0, -1.570, 3.141, 0, -1.570
]
#self.initialJointAnglesSideOfBodyLEFT = [-1.570, 0, 0, -1.570, 3.141, 0, -1.570]
self.initialJointAnglesSideFacingFowardLEFT = [
1.57, 0, 1.57, -1.57, 1.57, 0, -1.57
]
self.initialJointAnglesSideOfBodyRIGHT = [
-1.570, 0, 0, -1.570, 3.141, 0, -4.712
]
self.initialJointAnglesSideFacingFowardRIGHT = [
1.570, 0, 0, -1.570, 1.570, -1.570, -4.712
]
#Variables...! #
armReachAction.iteration = 0
self.bowlPosOffsets = np.array([
[-.01, 0, .4], #offsets from bowl_pos to left arm spoon
[-.01, 0, .008], #scooping motion is set of offsets
[.05, 0, .008],
[.05, 0, .008],
[.02, 0, .6],
[0, 0, 0],
[.02, 0, .6]
])
self.bowlEulers = np.array([
[90, -60, 0], #Euler angles, XYZ rotations for left arm spoon
[90, -60, 0], #controls scooping motion
[90, -60, 0],
[90, -30, 0],
[90, 0, 0],
[0, 0, 0],
[90, 0, 0]
])
self.headPosOffsets = np.array([
[.01, .085, -.01], #offsets from head_frame to left arm spoon
[.01, .2, .1], #feeding motion set of offsets
[0, 0, 0]
])
self.headEulers = np.array([
[90, 0, -90], #Euler angles, XYZ rotations for left arm spoon
[90, 0, -45], #controls feeding to mouth motion
[90, 0, 0]
])
self.stopPos = np.array([[.7, .7, .5]])
self.stopEulers = np.array([[90, 0, 30]])
self.rightArmPosOffsets = np.array(
[[.02, .05, -.01], [.02, .1, .3],
[0, 0, 0]]) #Set of pos offsets for the right arm end effector
self.rightArmEulers = np.array(
[[90, 0, 0], [90, 0, 0],
[0, 0, 0]]) #Set of end effector angles for right arm
self.kinectBowlFoundPosOffsets = [-.08, -.04, 0]
self.timeouts = [20, 7, 4, 4, 4, 10, 7]
self.timeoutsR = [10, 10, 10]
self.kinectReachTimeout = 15
self.bowlQuatOffsets = self.euler2quatArray(
self.bowlEulers
) #converts the array of eulers to an array of quats
self.headQuatOffsets = self.euler2quatArray(self.headEulers)
self.rightArmQuatOffsets = self.euler2quatArray(self.rightArmEulers)
self.stopQuatOffsets = self.euler2quatArray(self.stopEulers)
print "Calculated quaternions:"
print self.bowlQuatOffsets
try:
print "--------------------------------"
raw_input("Register bowl & head position! Then press Enter \m")
#self.tf_lstnr.waitForTransform('/torso_lift_link', 'head_frame', rospy.Time.now(), rospy.Duration(10))
(self.headPos, self.headQuat) = self.tf_lstnr.lookupTransform(
'/torso_lift_link', 'head_frame', rospy.Time(0))
print "Recived head position: \n"
print self.headPos
print self.headQuat
print "--------------------------------"
raw_input("Press Enter to confirm.")
print "--------------------------------"
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "Oops, can't get head_frame tf info!, trying again :)"
rate = rospy.Rate(100) # 25Hz, nominally.
while not rospy.is_shutdown():
if self.getJointAngles() != []:
print "--------------------------------"
print "Current joint angles"
print self.getJointAngles()
print "Current pose"
print self.getEndeffectorPose()
print "--------------------------------"
break
rospy.spin()