def sendWalkingPlanRequest(self, footstepPlan, startPose, waitForResponse=False, waitTimeout=5000, req_type='traj'):
msg = lcmdrc.walking_plan_request_t()
msg.utime = getUtime()
state_msg = robotstate.drakePoseToRobotState(startPose)
msg.initial_state = state_msg
msg.new_nominal_state = msg.initial_state
msg.use_new_nominal_state = True
msg.footstep_plan = footstepPlan
if req_type == 'traj':
requestChannel = 'WALKING_TRAJ_REQUEST'
responseChannel = 'WALKING_TRAJ_RESPONSE'
response_type = lcmdrc.robot_plan_t
elif req_type == 'controller':
requestChannel = 'WALKING_CONTROLLER_PLAN_REQUEST'
responseChannel = 'WALKING_CONTROLLER_PLAN_RESPONSE'
response_type = lcmdrc.walking_plan_t
elif req_type == 'simulate_drake':
requestChannel = 'WALKING_SIMULATION_DRAKE_REQUEST'
responseChannel = 'WALKING_SIMULATION_TRAJ_RESPONSE'
response_type = lcmdrc.robot_plan_t
else:
raise ValueError("Invalid request type: {:s}".format(req_type))
if waitForResponse:
if waitTimeout == 0:
helper = lcmUtils.MessageResponseHelper(responseChannel, response_type)
lcmUtils.publish(requestChannel, msg)
return helper
return lcmUtils.MessageResponseHelper.publishAndWait(requestChannel, msg,
responseChannel, response_type, waitTimeout)
else:
lcmUtils.publish(requestChannel, msg)
def makePlanMessage(self, poses, poseTimes, info, fields):
from director import robotstate
# rescale poseTimes
poseTimes = np.array(poseTimes) * 2.0
states = [robotstate.drakePoseToRobotState(pose) for pose in poses]
for i, state in enumerate(states):
state.utime = poseTimes[i] * 1e6
msg = robot_plan_t()
msg.utime = fields.utime
msg.robot_name = 'robot'
msg.num_states = len(states)
msg.plan = states
msg.plan_info = [info] * len(states)
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 autoIRISSegmentation(self,
xy_lb=[-2, -2],
xy_ub=[2, 2],
max_num_regions=10,
default_yaw=np.nan,
max_slope_angle=np.nan,
max_height_variation=np.nan,
plane_distance_tolerance=np.nan,
plane_angle_tolerance=np.nan):
msg = lcmdrc.auto_iris_segmentation_request_t()
msg.utime = getUtime()
A = np.vstack((np.eye(2), -np.eye(2)))
b = np.hstack((xy_ub, -np.asarray(xy_lb)))
msg.xy_bounds = irisUtils.encodeLinCon(A, b)
msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
msg.num_seed_poses = 0
msg.max_num_regions = max_num_regions
msg.region_id = self.getNewUIDs(max_num_regions)
msg.default_yaw = default_yaw
msg.max_slope_angle = max_slope_angle
msg.max_height_variation = max_height_variation
msg.plane_distance_tolerance = plane_distance_tolerance
msg.plane_angle_tolerance = plane_angle_tolerance
lcmUtils.publish('AUTO_IRIS_SEGMENTATION_REQUEST', msg)
def makePlanMessage(self, poses, poseTimes, info, fields):
from director import robotstate
# rescale poseTimes
poseTimes = np.array(poseTimes)*2.0
states = [robotstate.drakePoseToRobotState(pose) for pose in poses]
for i, state in enumerate(states):
state.utime = poseTimes[i]*1e6
msg = robot_plan_t()
msg.utime = fields.utime
msg.robot_name = 'robot'
msg.num_states = len(states)
msg.plan = states
msg.plan_info = [info]*len(states)
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 autoIRISSegmentation(self,
xy_lb=[-2, -2],
xy_ub=[2, 2],
max_num_regions=10,
default_yaw=np.nan,
max_slope_angle=np.nan,
max_height_variation=np.nan,
plane_distance_tolerance=np.nan,
plane_angle_tolerance=np.nan):
msg = lcmdrc.auto_iris_segmentation_request_t()
msg.utime = getUtime()
A = np.vstack((np.eye(2), -np.eye(2)))
b = np.hstack((xy_ub, -np.asarray(xy_lb)))
msg.xy_bounds = irisUtils.encodeLinCon(A, b)
msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
msg.num_seed_poses = 0
msg.max_num_regions = max_num_regions
msg.region_id = self.getNewUIDs(max_num_regions)
msg.default_yaw = default_yaw
msg.max_slope_angle = max_slope_angle
msg.max_height_variation = max_height_variation
msg.plane_distance_tolerance = plane_distance_tolerance
msg.plane_angle_tolerance = plane_angle_tolerance
lcmUtils.publish('AUTO_IRIS_SEGMENTATION_REQUEST', msg)
def _updateAndSendCommandMessage(self):
self._currentCommandedPose = self.clipPoseToJointLimits(self._currentCommandedPose)
if self._baseFlag:
msg = robotstate.drakePoseToRobotState(self._currentCommandedPose)
else:
msg = drakePoseToAtlasCommand(self._currentCommandedPose)
if self.useControllerFlag:
msg = drakePoseToQPInput(self._currentCommandedPose)
lcmUtils.publish(self.publishChannel, 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 requestIRISRegion(self, tform, uid, bounding_box_width=2):
start = np.asarray(tform.GetPosition())
A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width)
msg = lcmdrc.iris_region_request_t()
msg.utime = getUtime()
msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
msg.num_seed_poses = 1
msg.seed_poses = [positionMessageFromFrame(tform)]
msg.region_id = [uid]
msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
lcmUtils.publish(self.request_channel, msg)
def _updateAndSendCommandMessage(self):
self._currentCommandedPose = self.clipPoseToJointLimits(self._currentCommandedPose)
if self._baseFlag:
msg = robotstate.drakePoseToRobotState(self._currentCommandedPose)
else:
msg = drakePoseToAtlasCommand(self._currentCommandedPose)
if self.useControllerFlag:
msg = drakePoseToQPInput(self._currentCommandedPose, useConstrainedDofs=self.options['useConstrainedDofs'],
fixedBase=self.options['fixedBase'], forceControl=self.options['forceControl'])
lcmUtils.publish(self.publishChannel, 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 _updateAndSendCommandMessage(self):
self._currentCommandedPose = self.clipPoseToJointLimits(
self._currentCommandedPose)
if self._baseFlag:
msg = robotstate.drakePoseToRobotState(self._currentCommandedPose)
else:
msg = drakePoseToAtlasCommand(self._currentCommandedPose)
if self.useControllerFlag:
msg = drakePoseToQPInput(self._currentCommandedPose)
lcmUtils.publish(self.publishChannel, msg)
def requestIRISRegion(self, tform, uid, bounding_box_width=2):
start = np.asarray(tform.GetPosition())
A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width)
msg = lcmdrc.iris_region_request_t()
msg.utime = getUtime()
msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
msg.num_seed_poses = 1
msg.seed_poses = [positionMessageFromFrame(tform)]
msg.region_id = [uid]
msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
lcmUtils.publish(self.request_channel, msg)
def constructFootstepPlanRequest(self, pose, goalFrame=None):
msg = lcmdrc.footstep_plan_request_t()
msg.utime = getUtime()
state_msg = robotstate.drakePoseToRobotState(pose)
msg.initial_state = state_msg
if goalFrame is None:
goalFrame = vtk.vtkTransform()
msg.goal_pos = positionMessageFromFrame(goalFrame)
msg = self.applyParams(msg)
msg = self.applySafeRegions(msg)
return msg
def publishEstRobotState(self):
q = self.robotSystem.robotStateJointController.q
stateMsg = robotstate.drakePoseToRobotState(q)
stateMsg.utime = utimeUtil.getUtime()
lcmUtils.publish("EST_ROBOT_STATE", stateMsg)
def onIiwaStatus(msg):
q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + list(msg.joint_position_measured)
stateMsg = robotstate.drakePoseToRobotState(q)
stateMsg.utime = msg.utime
lcmUtils.publish('EST_ROBOT_STATE', stateMsg)
def sendEstRobotState(pose=None):
if pose is None:
pose = robotStateJointController.q
msg = robotstate.drakePoseToRobotState(pose)
lcmUtils.publish('EST_ROBOT_STATE', msg)
def onIiwaStatus(msg):
q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + list(msg.joint_position_measured)
stateMsg = robotstate.drakePoseToRobotState(q)
stateMsg.utime = msg.utime
lcmUtils.publish("EST_ROBOT_STATE", stateMsg)
def sendEstRobotState(pose=None):
if pose is None:
pose = robotStateJointController.q
msg = robotstate.drakePoseToRobotState(pose)
lcmUtils.publish('EST_ROBOT_STATE', msg)