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 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 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 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 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