def createArmCmd():
global cmdx, cmdy, cmdn, cmdd, cmds, cmde, cmdl
#[ 'ShP', 'ShR', 'ShY', 'ElP', 'FoY', 'WrR', 'WrP']
ZERO_VECTOR = [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0]
ELBOW_BENT_UP = [0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0]
REACHING_UP = [0.0, 0.0, -1.5, 2.0, 0.0, 0.0, 0.0]
#REACH_SWITCH = [-1.35, 0.9, 1.5, 0.0, 0.0, 0.0, 0.0]
REACH_SWITCH = [-1.35, 0.95, 1.5, 0.0, 0.0, 0.0, 0.0]
RETRACT = [-1.0, 0.0, 1.5, 1.5, 0.0, 0.0, 0.0]
HOME_VECTOR = [-0.2, 1.2, 0.7, 1.5, 0.0, 0.0, 0.0]
ARM_STRAIGHT = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
msg = appendTrajectoryPoint(msg, 2.0, ARM_STRAIGHT)
msg = appendTrajectoryPoint(msg, 3.0, REACHING_UP)
msg = appendTrajectoryPoint(msg, 4.0, REACH_SWITCH)
msg = appendTrajectoryPoint(msg, cmdl + 5.0, REACH_SWITCH)
msg = appendTrajectoryPoint(msg, cmdl + 5.5, RETRACT)
msg = appendTrajectoryPoint(msg, cmdl + 6.0, HOME_VECTOR)
msg.unique_id = -1
return msg, " arm linger for %6.3fs" % (cmdl)
def pushButtonHardCode(self, in_msg):
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
# ELBOW_BENT_UP = [0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0]
# to understand this, generate a frame list; these joints are in that order
# the first is shoulder roll; last is wrist roll
# if invalid values are used, the message silently fails.
#setup_vector1 = [+numpy.pi/6, 1.5, 0.0, 2.0, 0.0, 0.0, 0.0]
setup_vector2 = [+numpy.pi / 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
##button_push = [-1.151, .930, 1.110, 0.624, 0.0, 0.0, 0.0]
button_push = [-1.151, .75, 0.7, 0.624, 0.0, 0.0, 0.0]
rest_state = [0.0, 1.5, 0.0, 2.0, 0.0, 0.0, 0.0]
# The numbers represent time by which this motion should be completed
# speeding up the part before button_push vector has an advantage, the others
# not (because we are waiting for the door to open anyway
#msg = self.appendArmTrajectoryPoint(msg, 0.5, setup_vector1)
# msg = self.appendArmTrajectoryPoint(msg, 1.0, setup_vector2)
# msg = self.appendArmTrajectoryPoint(msg, 1.5, button_push)
# msg = self.appendArmTrajectoryPoint(msg, 2.0, setup_vector2)
# msg = self.appendArmTrajectoryPoint(msg, 2.5, rest_state)
msg = self.appendArmTrajectoryPoint(msg, 0.5, setup_vector2)
msg = self.appendArmTrajectoryPoint(msg, 1.0, button_push)
msg = self.appendArmTrajectoryPoint(msg, 1.5, rest_state)
msg.execution_mode = ArmTrajectoryRosMessage.OVERRIDE
msg.unique_id = rospy.Time.now().nsecs
rospy.loginfo('publishing right trajectory')
self.armTrajectoryPublisher.publish(msg)
def sendRightArm(self, target):
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
if target == "home":
msg = self.appendTrajectoryPoint(msg, 2.0, HOME_POSITION)
elif target == "button":
msg = self.appendTrajectoryPoint(msg, 2.0, BUTTON_POSITION)
elif target == "down":
msg = self.appendTrajectoryPoint(msg, 2.0, DOWN_POSITION)
elif target == "extend":
msg = self.appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
elif target == "tight":
msg = self.appendTrajectoryPoint(msg, 2.0, IN_TIGHT)
elif target == "back":
msg = self.appendTrajectoryPoint(msg, 2.0, BACK_POSITION)
else:
#rospy.loginfo('Failed to send arm to ' + target + ' position')
#return 1
msg = self.appendTrajectoryPoint(msg, 2.0, target)
msg.unique_id = -1
if isinstance(target, basestring):
strtarget = target
else:
strtarget = str(target)
rospy.loginfo('sending right arm to ' + strtarget + ' position')
self.publish(msg)
return 0
def sendLeftArm(self, target):
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.LEFT
if target == "home":
msg = self.appendTrajectoryPoint(msg, 2.0, makeLeft(HOME_POSITION))
elif target == "button":
msg = self.appendTrajectoryPoint(msg, 2.0,
makeLeft(BUTTON_POSITION))
elif target == "down":
msg = self.appendTrajectoryPoint(
msg, 2.0, makeLeft(DOWN_POSITION)
) # [0.0, -1.3, 0.0, 0.0, 0.0, 0.0, 0.0]) # DOWN_POSITION)
elif target == "extend":
msg = self.appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
elif target == "tight":
msg = self.appendTrajectoryPoint(msg, 2.0, makeLeft(IN_TIGHT))
else:
#rospy.loginfo('Failed to send arm to ' + target + ' position')
#return 1
msg = self.appendTrajectoryPoint(msg, 2.0, makeLeft(target))
msg.unique_id = -1
if isinstance(target, basestring):
strtarget = target
else:
strtarget = str(target)
rospy.loginfo('sending left arm to ' + strtarget + ' position')
self.publish(msg)
return 0
def ArmMsgMaker(time, side, POSITION):
msg = ArmTrajectoryRosMessage()
msg.execution_mode = 0
msg.robot_side = side
msg.unique_id = -1
msg = appendTrajectoryPoint(msg, time, POSITION)
armTrajectoryPublisher.publish(msg)
def sendLeftArmDown():
global uid
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.LEFT
msg = appendTrajectoryPoint(msg, 0.5, LOWER_ARM_LF)
msg.unique_id = time.time()
rospy.loginfo('publishing left dn trajectory')
armTrajectoryPublisher.publish(msg)
def sendLeftArmTrajectory():
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.LEFT
msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
msg = appendTrajectoryPoint(msg, 4.0, LEFT_HOME)
msg.unique_id = -1
rospy.loginfo('publishing left trajectory')
armTrajectoryPublisher.publish(msg)
def sendLeftArmTrajectory():
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.LEFT
msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
msg = appendTrajectoryPoint(msg, 4.0, LEFT_HOME)
msg.unique_id = -1
rospy.loginfo('publishing left trajectory')
armTrajectoryPublisher.publish(msg)
def sendRightArmDown():
global uid
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
msg = appendTrajectoryPoint(msg, 0.5, LOWER_ARM)
uid +=1
msg.unique_id = time.time()
rospy.loginfo('publishing right dn trajectory')
armTrajectoryPublisher.publish(msg)
def sendRightArmTrajectory():
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
msg = appendTrajectoryPoint(msg, 3.0, ZERO_VECTOR)
msg = appendTrajectoryPoint(msg, 4.0, ELBOW_BENT_UP)
msg = appendTrajectoryPoint(msg, 5.0, ZERO_VECTOR)
msg.unique_id = -1
rospy.loginfo('publishing right trajectory')
armTrajectoryPublisher.publish(msg)
def sendRightArmTrajectory():
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
msg = appendTrajectoryPoint(msg, 3.0, ELBOW_BENT_UP)
msg = appendTrajectoryPoint(msg, 4.0, ZERO_VECTOR)
msg = appendTrajectoryPoint(msg, 6.0, RIGHT_HOME)
msg.unique_id = -1
rospy.loginfo('publishing right trajectory')
armTrajectoryPublisher.publish(msg)
def createArmCmd():
global cmdx, cmdy, cmdn, cmdd, cmds, cmde, cmdl, cmdb, cmdw
#[ 'ShP', 'ShR', 'ShY', 'ElP', 'FoY', 'WrR', 'WrP']
ZERO_VECTOR = [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0]
ELBOW_BENT_UP = [0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0]
REACHING_UP = [0.0, 0.0, -1.5, 2.0, 0.0, 0.0, 0.0]
#REACH_SWITCH = [-1.35, 0.9, 1.5, 0.0, 0.0, 0.0, 0.0]
REACH_SWITCH = [-1.35, 0.95, 1.5, 0.0, 0.0, 0.0, 0.0]
RETRACT = [-1.0, 0.0, 1.5, 1.5, 0.0, 0.0, 0.0]
HOME_VECTOR = [-0.2, 1.2, 0.7, 1.5, 0.0, 0.0, 0.0]
ARM_STRAIGHT = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
L_ARM_CLOSE = [-0.2, -1.4, 0.2, -1.5, 0.0, 0.0, 0.0]
L_ARM_OUT = [-0.9, -0.4, 0.2, -1.5, 0.0, 0.0, 0.0]
R_ARM_CLOSE = [-0.2, 1.4, 0.2, 1.5, 0.0, 0.0, 0.0]
R_ARM_OUT = [-0.9, 0.4, 0.2, 0.6, 0.0, 0.0, 0.0]
msgl = ArmTrajectoryRosMessage()
msgl.robot_side = ArmTrajectoryRosMessage.LEFT
msgl = appendTrajectoryPoint(msgl, 4.0, L_ARM_OUT)
msgl = appendTrajectoryPoint(msgl, cmdl + 5.0, L_ARM_OUT)
msgl = appendTrajectoryPoint(msgl, cmdl + 5.2, L_ARM_CLOSE)
msgl.unique_id = -1
msgr = ArmTrajectoryRosMessage()
msgr.robot_side = ArmTrajectoryRosMessage.RIGHT
msgr = appendTrajectoryPoint(msgr, 4.0, R_ARM_OUT)
msgr = appendTrajectoryPoint(msgr, cmdl + 5.0, R_ARM_OUT)
msgr = appendTrajectoryPoint(msgr, cmdl + 5.2, R_ARM_CLOSE)
msgr.unique_id = -1
return msgl, msgr, " arms linger for %6.3fs" % (cmdl)
def returnHome(self, in_msg):
msg = ArmTrajectoryRosMessage()
msg.robot_side = in_msg.data
if self.LEFT == in_msg.data:
rest_state = [0.0, -1.5, 0.0, -2.0, 0.0, 0.0, 0.0]
else:
rest_state = [0.0, 1.5, 0.0, 2.0, 0.0, 0.0, 0.0]
msg = self.appendArmTrajectoryPoint(msg, 1.0, rest_state)
msg.execution_mode = ArmTrajectoryRosMessage.OVERRIDE
msg.unique_id = rospy.Time.now().nsecs
rospy.loginfo('publishing right trajectory')
self.armTrajectoryPublisher.publish(msg)
def process_arm_command(self, binding, ch):
msg = ArmTrajectoryRosMessage()
msg.unique_id = -1
side = binding['side']
if side == 'left':
msg.robot_side = ArmTrajectoryRosMessage.LEFT
elif side == 'right':
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
else:
assert False, "Unknown arm side '%s'" % side
self._update_joint_values('%s arm' % side, binding, ch)
self._append_trajectory_point_1d(msg, 1.0,
self.joint_values['%s arm' % side])
self.arm_publisher.publish(msg)
def sendRightArmTrajectory():
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
# msg = appendTrajectoryPoint(msg, 3.0, ELBOW_BENT_UP)
# msg = appendTrajectoryPoint(msg, 1.0, ZERO_VECTOR)
# msg = appendTrajectoryPoint(msg, 4.0, ZERO_VECTOR)
# msg = appendTrajectoryPoint(msg, 2.0, BUTTON_PRESS)
msg.unique_id = -1
rospy.loginfo('publishing right trajectory')
armTrajectoryPublisher.publish(msg)
time.sleep(0.5)
msg.robot_side = ArmTrajectoryRosMessage.LEFT
armTrajectoryPublisher.publish(msg)
def process_arm_command(self, binding, data):
msg = ArmTrajectoryRosMessage()
msg.unique_id = -1
side = binding['side']
if side == 'left':
msg.robot_side = ArmTrajectoryRosMessage.LEFT
elif side == 'right':
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
else:
assert False, "Unknown arm side '%s'" % side
self._update_joint_values_d('%s arm' % side, binding, data)
if binding['joint_index'] == 'reset': time = 1.0
else: time = data.get_sibling("tim").get_rosval()
self._append_trajectory_point_1d(msg, time,
self.joint_values['%s arm' % side])
self.arm_publisher.publish(msg)
def sendTrajectory(joint_waypoints):
msgs = {}
msgs["left"] = copy.deepcopy(ArmTrajectoryRosMessage())
msgs["right"] = copy.deepcopy(ArmTrajectoryRosMessage())
msgs["left"].robot_side = ArmTrajectoryRosMessage.LEFT
msgs["right"].robot_side = ArmTrajectoryRosMessage.RIGHT
# for each set of joint states
for y in joint_waypoints:
# first value is time duration
time = float(y[0])
side = str(y[1]).lower()
# subsequent values are desired joint positions
commandPosition = array([float(x) for x in y[2].split()])
if side in msgs.keys():
msgs[side] = appendTrajectoryPoint(msgs[side], time,
commandPosition)
msgs[side].unique_id = -1
for side in msgs.keys():
if msgs[side].unique_id == -1:
rospy.loginfo('publishing %s trajectory' % side)
armTrajectoryPublisher.publish(msgs[side])
def sendRightArmTrajectory():
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
msg = appendTrajectoryPoint(msg, 2.0, ARM_STRAIGHT)
#msg = appendTrajectoryPoint(msg, 4.0, ELBOW_BENT_UP)
#msg = appendTrajectoryPoint(msg, 8.0, REACHING_UP)
#msg = appendTrajectoryPoint(msg, 1.0, RETRACT)
#msg = appendTrajectoryPoint(msg, 4.5, RETRACT)
#msg = appendTrajectoryPoint(msg, 2.0, REACH_SWITCH)
msg = appendTrajectoryPoint(msg, 3.0, REACHING_UP)
msg = appendTrajectoryPoint(msg, 4.0, REACH_SWITCH)
msg = appendTrajectoryPoint(msg, dwell + 3.0, REACH_SWITCH)
msg = appendTrajectoryPoint(msg, dwell + 3.5, RETRACT)
msg = appendTrajectoryPoint(msg, dwell + 4.0, HOME_VECTOR)
msg.unique_id = -1
rospy.loginfo('publishing right trajectory')
armTrajectoryPublisher.publish(msg)
def sendRightArmTrajectory():
global uid
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
#msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
#msg = appendTrajectoryPoint(msg, 2.0, ELBOW_BENT_UP)
msg = appendTrajectoryPoint(msg, 0.5, ARM_UP0)
time.sleep(2)
msg = appendTrajectoryPoint(msg, 0.5, ARM_UP2)
time.sleep(2)
#msg = appendTrajectoryPoint(msg, 0.5, ARM_UP2)
#time.sleep(2)
#msg = appendTrajectoryPoint(msg, 1.0, PUNCH)
uid -= 1
msg.unique_id = uid
print uid
rospy.loginfo('publishing right trajectory')
armTrajectoryPublisher.publish(msg)
def sendRightArmTrajectory():
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
test = inverseKinematics(chi)
test1, test2, test3, test4 = test
test1 = test1[0]
test2 = test2[0]
test3 = test3[0]
test4 = test4[0]
test = [test1, test2, test3, test4]
print test
ZERO_VECTOR = [test[0], test[1], test[2], test[3], 0, 0, 0]
msg = appendTrajectoryPoint(msg, 4.0, ZERO_VECTOR)
msg.unique_id = -1
rospy.loginfo('publishing right trajectory')
armTrajectoryPublisher.publish(msg)
def make_arm_trajectory(side, mode, time, joints, previous_msg = None):
trajectories = []
for x in joints:
trajectories.append(makeJoint(time, x))
msg = ArmTrajectoryRosMessage()
msg.robot_side = side
msg.joint_trajectory_messages = trajectories
msg.execution_mode = mode
msg.unique_id = uid()
if previous_msg is not None:
msg.previous_message_id = previous_msg
return msg
def sendRightArmTrajectory():
with open("tracefile_l.txt") as infile:
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.LEFT
msg.unique_id = -1
msg.execution_mode = 0
for line in infile:
time = 2.0
# if i == 1:
# #msg = appendTrajectoryPoint(msg, time, [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0])
# time = time + 1.0
# else:
# msg = ArmTrajectoryRosMessage()
# msg.robot_side = ArmTrajectoryRosMessage.LEFT
# msg.unique_id = -1
# msg.execution_mode = 0
# msg.previous_message_id = 0
elems = line.split(' ')
traj = []
traj.append(float(elems[1]))
traj.append(-float(elems[2]))
traj.append(float(elems[3]))
traj.append(float(elems[4]))
traj.append(float(elems[5]))
traj.append(float(elems[6]))
traj.append(float(elems[7]))
#print(traj)
"""
left elbow pitch issues
"""
if traj[3] < -2.174:
traj[3] = -2.174
if traj[3] > 0.12:
traj[3] = 0.12
"""
left shoulder roll issues
"""
if traj[1] < -1.519:
traj[1] = -1.519
if traj[1] > 1.266:
traj[1] = 1.266
"""
left shoudler yaw issues:
"""
if traj[2] < -3.1:
traj[2] = -3.1
if traj[2] > 2.18:
traj[2] = 2.18
"""
left shoudler pitch issues:
"""
if traj[0] < -2.85:
traj[0] = -2.85
if traj[0] > 2.0:
traj[0] = 2.0
"""
left forearm yaw issues
"""
if traj[4] < -2.019:
traj[4] = -2.019
if traj[4] > 3.14:
traj[4] = 3.14
msg = appendTrajectoryPoint(msg, time, traj)
time = time + 500.0
attempts = 0
while attempts < 5:
try:
armTrajectoryPublisher.publish(msg)
time.sleep(1)
print('publised left message')
i = i + 1
attempts = 6
except:
print("msg left failed, ignoring")
attempts = attempts + 1
with open("tracefile_r.txt") as infile:
msg = ArmTrajectoryRosMessage()
msg.robot_side = ArmTrajectoryRosMessage.RIGHT
msg.unique_id = -1
msg.execution_mode = 0
for line in infile:
time = 2.0
# if i == 1:
# #msg = appendTrajectoryPoint(msg, time, [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0])
# time = time + 1.0
# else:
# msg = ArmTrajectoryRosMessage()
# msg.robot_side = ArmTrajectoryRosMessage.LEFT
# msg.unique_id = -1
# msg.execution_mode = 0
# msg.previous_message_id = 0
elems = line.split(' ')
traj = []
traj.append(float(elems[1]))
traj.append(float(elems[2]))
traj.append(float(elems[3]))
traj.append(float(elems[4]))
traj.append(float(elems[5]))
traj.append(float(elems[6]))
traj.append(float(elems[7]))
#print(traj)
"""
right elbow pitch issues
"""
if traj[3] < -0.12:
traj[3] = -0.12
if traj[3] > 2.174:
traj[3] = 2.174
"""
right shoulder roll issues
"""
if traj[1] < -1.226:
traj[1] = -1.226
if traj[1] > 1.519:
traj[1] = 1.519
"""
right shoudler yaw issues:
"""
if traj[2] < -3.1:
traj[2] = -3.1
if traj[2] > 2.18:
traj[2] = 2.18
"""
right forearm yaw issues
"""
if traj[4] < -2.019:
traj[4] = -2.019
if traj[4] > 3.14:
traj[4] = 3.14
"""
right sh pitch issues
"""
if traj[0] < -2.85:
traj[0] = -2.85
if traj[0] > 2.0:
traj[0] = 2.0
msg = appendTrajectoryPoint(msg, time, traj)
time = time + 500.0
attempts = 0
while attempts < 3:
try:
armTrajectoryPublisher.publish(msg)
time.sleep(1)
print('publised right message')
i = i + 1
attempts = 6
except:
print("msg right failed, ignoring")
attempts = attempts + 1