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)