def appendPelvisPoint(pelvis_msg, time, position):
if not pelvis_msg.trajectory_points:
pelvis_msg.trajectory_points = [
copy.deepcopy(TrajectoryPoint1DRosMessage())
for i in range(len(pelvis_msg.trajectory_points))
]
point = TrajectoryPoint1DRosMessage()
point.time = time
point.position = position
point.velocity = 0
pelvis_msg.trajectory_points.append(point)
return pelvis_msg
def glove_to_robot_node():
unid = 1
rospy.init_node('glove_to_robot', anonymous=True)
rate = rospy.Rate(100)
pub = rospy.Publisher('/arm_control', HandPoseTrajectoryRosMessage, queue_size=1)
sub = rospy.Subscriber("/glove_status", JointState, glove_callback)
null_point1d = TrajectoryPoint1DRosMessage()
null_point1d.position = 0.5
null_onedof_1 = OneDoFJointTrajectoryRosMessage()
null_onedof_2 = OneDoFJointTrajectoryRosMessage()
null_onedof_3 = OneDoFJointTrajectoryRosMessage()
right_msg = HandPoseTrajectoryRosMessage()
right_msg.robot_side = 1 ##RIGHT
right_msg.execution_mode = 0
right_msg.desired_pose = 0
right_msg.previous_message_id = unid - 1
right_msg.homeAllForearmJoints = True
right_troll = OneDoFJointTrajectoryRosMessage()
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
def _append_single_trajectory_point_1d(self, msg, time, joint_value):
if not msg.trajectory_points:
msg.trajectory_points = []
point = TrajectoryPoint1DRosMessage()
point.time = time
point.position = joint_value
point.velocity = 0
msg.trajectory_points.append(point)
def makeJoint(time, position):
point = TrajectoryPoint1DRosMessage()
point.time = time
point.position = position
joint = OneDoFJointTrajectoryRosMessage()
joint.trajectory_points = [point]
return joint
def appendTrajectoryPoint(self,neck_trajectory, time, positions):
if not neck_trajectory.joint_trajectory_messages:
neck_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
neck_trajectory.joint_trajectory_messages[i].trajectory_points.append(point)
return neck_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]
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)
def sendNeck(self):
msg = NeckTrajectoryRosMessage()
msg.unique_id = -1
msg.joint_trajectory_messages = []
point0 = TrajectoryPoint1DRosMessage()
point0.time = 1.0
point0.position = -0.3
point0.velocity = 0
point1 = TrajectoryPoint1DRosMessage()
point1.time = 1.0
point1.position = -0.3
point1.velocity = 0
point2 = TrajectoryPoint1DRosMessage()
point2.time = 1.0
point2.position = -0.3
point2.velocity = 0
msg.joint_trajectory_messages.trajectory_points.append([point0,point1,point2])
self.neck_publisher.publish(msg)
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 _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)
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 left_msg.unique_id = unid left_rin = last_msg.position[1] left_mid = last_msg.position[2] left_ind = last_msg.position[3] left_thumb = last_msg.position[4] left_rin = (left_rin * 2.7/3.0) + 0.3 left_mid = (left_mid * 2.7/3.0) + 0.3 left_ind = (left_ind * 2.7/3.0) + 0.3 left_thumb = (left_thumb * 1.7/3.0) + 0.3 left_pinky_point1d = TrajectoryPoint1DRosMessage() left_pinky_point1d.position = left_rin ##We use the value from the real life ring finger to control the robot's pinky left_pinky.trajectory_points = [left_pinky_point1d] left_middle_point1d = TrajectoryPoint1DRosMessage() left_middle_point1d.position = left_mid left_middle.trajectory_points = [left_middle_point1d] left_index_point1d = TrajectoryPoint1DRosMessage() left_index_point1d.position = left_ind left_index.trajectory_points = [left_index_point1d] left_thumb_point1d = TrajectoryPoint1DRosMessage() left_thumb_point1d.position = left_thumb left_tdist.trajectory_points = [left_thumb_point1d]
