def appendTrajectoryPointHand(hand_trajectory, time, position):
if not hand_trajector.joint_trajectory_messages:
hand_trajectory.hand_trajectory_messgaes = [
copy.deepcopy(OneDoFJointTrajectoryRosMessage())
for i in range(len(positions))
]
for i, pos in enumerate(positions):
point = TrajectoryPoint1DRosMessage()
point.time = time
point.position = pose
point.velocity = 0
hand_trajectory.joint_trajectory_messages[i].trajectory_points.append(
point)
return hand_trajectory
def appendTrajectoryPoint(self, arm_trajectory, time, positions):
if not arm_trajectory.joint_trajectory_messages:
arm_trajectory.joint_trajectory_messages = [
copy.deepcopy(OneDoFJointTrajectoryRosMessage())
for i in range(len(positions))
]
for i, pos in enumerate(positions):
point = TrajectoryPoint1DRosMessage()
point.time = time
point.position = pos
point.velocity = 0
arm_trajectory.joint_trajectory_messages[
i].trajectory_points.append(point)
return arm_trajectory
def _append_trajectory_point_1d(self, msg, time, joint_values):
if not msg.joint_trajectory_messages:
msg.joint_trajectory_messages = [
OneDoFJointTrajectoryRosMessage() for _ in joint_values]
if ON_REAL_ROBOT_USE:
for odjTm in msg.joint_trajectory_messages:
odjTm.weight = float('NaN')
for i, joint_value in enumerate(joint_values):
point = TrajectoryPoint1DRosMessage()
point.time = time
point.position = joint_value
point.velocity = 0
msg.joint_trajectory_messages[i].trajectory_points.append(point)
right_tprox = OneDoFJointTrajectoryRosMessage()
right_tdist = OneDoFJointTrajectoryRosMessage()
right_index = OneDoFJointTrajectoryRosMessage()
right_middle = OneDoFJointTrajectoryRosMessage()
right_pinky = OneDoFJointTrajectoryRosMessage()
right_fingers_list = [right_troll, right_tprox, right_tdist, right_index, right_middle, right_pinky]
right_msg.hand_joint_trajectory_messages = left_fingers_list
right_msg.forearm_joint_trajectory_messages = [null_onedof_1, null_onedof_2, null_onedof_3]
left_msg = HandPoseTrajectoryRosMessage()
left_msg.robot_side = 2 ##Double check that 2 is left
left_msg.execution_mode = 0
left_msg.desired_pose = 0
left_msg.previous_message_id = unid - 1
left_msg.homeAllForearmJoints = True
left_troll = OneDoFJointTrajectoryRosMessage()
left_tprox = OneDoFJointTrajectoryRosMessage()
left_tdist = OneDoFJointTrajectoryRosMessage()
left_index = OneDoFJointTrajectoryRosMessage()
left_middle = OneDoFJointTrajectoryRosMessage()
left_pinky = OneDoFJointTrajectoryRosMessage()
left_fingers_list = [left_troll, left_tprox, left_tdist, left_index, left_middle, left_pinky]
left_msg.hand_joint_trajectory_messages = left_fingers_list
left_msg.forearm_joint_trajectory_messages = [null_onedof_1, null_onedof_2, null_onedof_3]
for finger in left_fingers_list:
finger.trajectory_points.append(null_point1d)
for finger in right_fingers_list:
finger.trajectory_points.append(null_point1d)
while not rospy.is_shutdown(): ##There's a better way to do this than having pretty much the same code twice, shall be changed one day