def lower_head():
msg = HeadTrajectoryRosMessage()
point = SO3TrajectoryPointRosMessage()
point.time = 1.0
point.orientation.x = -0.2
point.orientation.y = 0.0
point.orientation.z = -0.0
point.orientation.w = 1.0
point.unique_id = time.time()
point.angular_velocity.x = 0.0
point.angular_velocity.y = 0.0
point.angular_velocity.z = 0.05
msg.taskspace_trajectory_points.append(point)
msg.unique_id = time.time()
#msg.unique_id = uid
#msg.previous_message_id = msg.unique_id+1
#msg.execution_mode = 0
print 'head trajectory ', msg
print time.time() + 1
print ''
rospy.loginfo('publishing head trajectory')
headTrajectoryPublisher.publish(msg)
def turn_chest():
msg = ChestTrajectoryRosMessage()
point = SO3TrajectoryPointRosMessage()
point.time = 1.0
point.orientation.x = 0.0
point.orientation.y = 0.0
point.orientation.z = 0.0
point.orientation.w = 1.0
point.unique_id = time.time()
point.angular_velocity.x = 0.0
point.angular_velocity.y = 0.0
point.angular_velocity.z = 0.05
msg.taskspace_trajectory_points.append(point)
msg.unique_id = time.time()
print 'chest trajectory ', msg
print ''
rospy.loginfo('publishing chest trajectory')
chestTrajectoryPublisher.publish(msg)
def _append_trajectory_point_so3(self, msg, time, joint_values):
roll, pitch, yaw = joint_values
quat = quaternion_from_euler(roll, pitch, yaw)
point = SO3TrajectoryPointRosMessage()
point.time = time
point.orientation = Quaternion()
point.orientation.x = quat[0]
point.orientation.y = quat[1]
point.orientation.z = quat[2]
point.orientation.w = quat[3]
point.angular_velocity = Vector3()
point.angular_velocity.x = 0
point.angular_velocity.y = 0
point.angular_velocity.z = 0
msg.taskspace_trajectory_points.append(point)
def appendTrajectorySO3(msg, time, rpy):
quat = quaternion_from_euler(rpy[0],rpy[1],rpy[2],)
point = SO3TrajectoryPointRosMessage()
point.time = time
point.orientation = Quaternion()
point.orientation.x = quat[0]
point.orientation.y = quat[1]
point.orientation.z = quat[2]
point.orientation.w = quat[3]
point.angular_velocity = Vector3()
point.angular_velocity.x = 0
point.angular_velocity.y = 0
point.angular_velocity.z = 0
msg.taskspace_trajectory_points.append(point)
return msg
def appendTrajectoryPoint(pelvis_orientation_trajectory, time, rollPitchYaw):
roll = rollPitchYaw[0]
pitch = rollPitchYaw[1]
yaw = rollPitchYaw[2]
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
point = copy.deepcopy(SO3TrajectoryPointRosMessage())
point.time = time
point.orientation = copy.deepcopy(Quaternion())
point.orientation.x = quat[0]
point.orientation.y = quat[1]
point.orientation.z = quat[2]
point.orientation.w = quat[3]
point.angular_velocity = copy.deepcopy(Vector3())
point.angular_velocity.x = 0
point.angular_velocity.y = 0
point.angular_velocity.z = 0
pelvis_orientation_trajectory.taskspace_trajectory_points.append(point)
return pelvis_orientation_trajectory