def convertKeyframePlan(self, keyframeMsg): msg = lcmdrc.robot_plan_t() msg.utime = keyframeMsg.utime msg.robot_name = keyframeMsg.robot_name msg.num_states = keyframeMsg.num_states msg.plan = keyframeMsg.plan msg.plan_info = keyframeMsg.plan_info msg.num_bytes = keyframeMsg.num_bytes msg.matlab_data = keyframeMsg.matlab_data msg.num_grasp_transitions = keyframeMsg.num_grasp_transitions msg.left_arm_control_type = msg.NONE msg.right_arm_control_type = msg.NONE msg.left_leg_control_type = msg.NONE msg.right_leg_control_type = msg.NONE return msg
def createPlanMessageFromPose(self, pose): msg = lcmdrc.robot_plan_t() msg.utime = 0.0 msg.robot_name = 'robot' msg.num_states = 2 state1 = robotstate.drakePoseToRobotState(pose) state2 = robotstate.drakePoseToRobotState(pose) state1.utime = 0.0 state2.utime = 0.01 msg.plan = [state1, state2] msg.plan_info = [1, 1] msg.num_bytes = 0 msg.matlab_data = [] msg.num_grasp_transitions = 0 msg.left_arm_control_type = msg.NONE msg.right_arm_control_type = msg.NONE msg.left_leg_control_type = msg.NONE msg.right_leg_control_type = msg.NONE return msg