コード例 #1
0
ファイル: footstepsdriver.py プロジェクト: tkoolen/director
    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)
コード例 #2
0
    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
コード例 #3
0
    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)
コード例 #4
0
ファイル: pydrakeik.py プロジェクト: openhumanoids/director
    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
コード例 #5
0
    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)
コード例 #6
0
    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)
コード例 #7
0
    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
コード例 #8
0
 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)
コード例 #9
0
    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)
コード例 #10
0
    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
コード例 #11
0
    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)
コード例 #12
0
 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)
コード例 #13
0
ファイル: footstepsdriver.py プロジェクト: tkoolen/director
    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
コード例 #14
0
ファイル: utils.py プロジェクト: sammy-tri/spartan-old
 def publishEstRobotState(self):
     q = self.robotSystem.robotStateJointController.q
     stateMsg = robotstate.drakePoseToRobotState(q)
     stateMsg.utime = utimeUtil.getUtime()
     lcmUtils.publish("EST_ROBOT_STATE", stateMsg)
コード例 #15
0
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)
コード例 #16
0
ファイル: startup.py プロジェクト: openhumanoids/director
def sendEstRobotState(pose=None):
    if pose is None:
        pose = robotStateJointController.q
    msg = robotstate.drakePoseToRobotState(pose)
    lcmUtils.publish('EST_ROBOT_STATE', msg)
コード例 #17
0
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)
コード例 #18
0
def sendEstRobotState(pose=None):
    if pose is None:
        pose = robotStateJointController.q
    msg = robotstate.drakePoseToRobotState(pose)
    lcmUtils.publish('EST_ROBOT_STATE', msg)