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 adjustChest(position, time): msg = ChestTrajectoryRosMessage() msg.unique_id = -1 msg.execution_mode = 0 msg.previous_message_id = 0 msg = appendTrajectoryPoint( msg, time, position) #Side to Side(+-20), Front back(+40), t.sleep(0.1) PC.chestTrajectoryPublisher.publish(msg)
def createChestCmd(self, vec): #Chest roll (never), pitch, yaw ZERO_VECTOR = [0.0, 0.0, 0.0] LEAN_FWD = [0.0, 0.3, 0.0] TURN_IN = [0.0, 0.3, 0.2] msg = ChestTrajectoryRosMessage() self._append_trajectory_point_so3(msg, 0.5, vec) msg.unique_id = -1 return msg
def createChestCmd(): global cmdx, cmdy, cmdn, cmdd, cmds, cmde, cmdl, cmdb, cmdw startingside = RIGHT if (cmdy < 0.0) else LEFT #Chest roll (never), pitch, yaw ZERO_VECTOR = [0.0, 0.0, 0.0] LEAN_FWD = [0.0, 0.3, 0.0] TURN_IN = [0.0, 0.3, 0.2] msg = ChestTrajectoryRosMessage() msg = appendTrajectorySO3(msg, 4.0, LEAN_FWD) msg = appendTrajectorySO3(msg, cmdl + 4.7, LEAN_FWD) msg = appendTrajectorySO3(msg, cmdl + 4.9, TURN_IN) msg = appendTrajectorySO3(msg, cmdl + 5.2, LEAN_FWD) msg.unique_id = -1 return msg, " chest"
def process_chest_command(self, binding, ch): msg = ChestTrajectoryRosMessage() msg.unique_id = -1 self._update_joint_values('chest', binding, ch) self._append_trajectory_point_so3(msg, 1.0, self.joint_values['chest']) self.chest_publisher.publish(msg)