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 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
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
