Exemple #1
0
    def updateCombinedStandLogic(self):
        if self.sentStandUtime is not None:
            if self.startupStage == 1:
                if getUtime() > self.sentStandUtime + 6e6:
                    # print "Sending SE Init"
                    self.sendInitAtZero()
                    self.startupStage = 2

            elif self.startupStage == 2:
                if getUtime() > self.sentStandUtime + 10e6:
                    self.sendBehaviorCommand("user")
                    self.sendMITStandCommand()
                    # print "Sending BDI User & MIT Stand commands"
                    self.startupStage = 0
Exemple #2
0
    def updateCombinedStandLogic(self):
        if (self.sentStandUtime is not None):
            if (self.startupStage == 1):
                if (getUtime() > self.sentStandUtime + 6E6):
                    # print "Sending SE Init"
                    self.sendInitAtZero()
                    self.startupStage = 2

            elif (self.startupStage == 2):
                if (getUtime() > self.sentStandUtime + 10E6):
                    self.sendBehaviorCommand('user')
                    self.sendMITStandCommand()
                    # print "Sending BDI User & MIT Stand commands"
                    self.startupStage = 0
Exemple #3
0
    def updateCombinedStandLogic(self):
        if (self.sentStandUtime is not None):
            if (self.startupStage == 1):
              if ( getUtime() > self.sentStandUtime + 6E6 ):
                  # print "Sending SE Init"
                  self.sendInitAtZero()
                  self.startupStage = 2

            elif (self.startupStage == 2):
              if ( getUtime() > self.sentStandUtime + 10E6 ):
                  self.sendBehaviorCommand('user')
                  self.sendMITStandCommand()
                  # print "Sending BDI User & MIT Stand commands"
                  self.startupStage = 0
Exemple #4
0
 def sendElectricArmEnabledState(self, enabledState):
     msg = lcmdrc.atlas_electric_arm_enable_t()
     msg.utime = getUtime()
     msg.num_electric_arm_joints = 6
     assert len(enabledState) == msg.num_electric_arm_joints
     msg.enable = enabledState
     lcmUtils.publish("ATLAS_ELECTRIC_ARM_ENABLE", msg)
def sendControlMessage(command, data=None, channel='DRILL_CONTROL'):
    m = drill_control_t()
    m.utime = getUtime()
    m.control_type = command
    m.data = data if data is not None else []
    m.data_length = len(m.data)
    lcmUtils.publish(channel, m)
Exemple #6
0
def sendControlMessage(command, data=None, channel='DRILL_CONTROL'):
    m = drill_control_t()
    m.utime = getUtime()
    m.control_type = command
    m.data = data if data is not None else []
    m.data_length = len(m.data)
    lcmUtils.publish(channel, m)
Exemple #7
0
    def sendFingerControl(self, positionA, positionB, positionC, force,
                          velocity, scissor, mode):
        assert 0.0 <= positionA <= 254.0
        assert 0.0 <= positionB <= 254.0
        assert 0.0 <= positionC <= 254.0
        assert 0.0 <= force <= 100.0
        assert 0.0 <= velocity <= 100.0
        assert 0 <= int(mode) <= 4

        if not scissor is None:
            assert 0.0 <= scissor <= 254.0

        msg = lcmrobotiq.command_t()
        msg.utime = getUtime()
        msg.activate = 1
        msg.emergency_release = 0
        msg.do_move = 1
        msg.ifc = 1
        msg.positionA = int(positionA)
        msg.positionB = int(positionB)
        msg.positionC = int(positionC)
        msg.force = int(254 * (force / 100.0))
        msg.velocity = int(254 * (velocity / 100.0))
        msg.mode = int(mode)

        if not scissor is None:
            msg.isc = 1
            msg.positionS = int(scissor)

        channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper()
        lcmUtils.publish(channel, msg)
	def publish(self):
		messages = self.reader.getMessages()
		
		for message in messages:
			channel = message[2]
			val = message[3] / 127.0
			if channel is 102:
				self.msg.slider1 = val
			elif channel is 103:
				self.msg.slider2 = val
			elif channel is 104:
				self.msg.slider3 = val
			elif channel is 105:
				self.msg.slider4 = val
			elif channel is 106:
				self.msg.knob1 = val
			elif channel is 107:
				self.msg.knob2 = val
			elif channel is 108:
				self.msg.knob3 = val
			elif channel is 109:
				self.msg.knob4 = val
			elif channel is 110:
				self.msg.knob5 = val
			elif channel is 111:
				self.msg.knob6 = val
			elif channel is 112:
				self.msg.knob7 = val
			elif channel is 113:
				self.msg.knob8 = val
		
		if len(messages) is not 0:
			self.msg.utime = getUtime()
			lcmUtils.publish(self.lcmChannel, self.msg)
Exemple #9
0
    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)
Exemple #10
0
    def sendDesiredPumpPsi(self, desiredPsi):
        msg = lcmdrc.atlas_pump_command_t()
        msg.utime = getUtime()

        msg.k_psi_p = 0.0  # Gain on pressure error (A/psi)
        msg.k_psi_i = 0.0  # Gain on the integral of the pressure error (A/(psi/s)
        msg.k_psi_d = 0.0  # Gain on the derivative of the pressure error (A/(psi s)

        msg.k_rpm_p = 0.0  # Gain on rpm error (A / rpm)
        msg.k_rpm_i = 0.0  # Gain on the integral of the rpm error (A / (rpm s))
        msg.k_rpm_d = 0.0  # Gain on the derivative of the rpm error (A / (rpm/s)

        msg.ff_rpm_d = 0.0  # Feed-forward gain on the desired rpm (A / rpm)
        msg.ff_psi_d = 0.0  # Feed-forward gain on the desired pressure (A / psi)
        msg.ff_const = 0.0  # Constant current term (Amps)

        msg.psi_i_max = 0.0  # Max. abs. value to which the integral psi error is clamped (psi s)
        msg.rpm_i_max = 0.0  # Max. abs. value to which the integral rpm error is clamped (rpm s)

        # Max. command output (A). Default is 60 Amps.
        # This value may need to be lower than the default in order to avoid
        # causing the motor driver to fault.
        msg.cmd_max = 60

        msg.desired_psi = desiredPsi  # default should be 1500
        msg.desired_rpm = 5000  # default should be 5000

        lcmUtils.publish("ATLAS_PUMP_COMMAND", msg)
Exemple #11
0
 def sendElectricArmEnabledState(self, enabledState):
     msg = lcmdrc.atlas_electric_arm_enable_t()
     msg.utime = getUtime()
     msg.num_electric_arm_joints = 6
     assert len(enabledState) == msg.num_electric_arm_joints
     msg.enable = enabledState
     lcmUtils.publish('ATLAS_ELECTRIC_ARM_ENABLE', msg)
Exemple #12
0
    def sendDesiredPumpPsi(self, desiredPsi):
        msg = lcmdrc.atlas_pump_command_t()
        msg.utime = getUtime()

        msg.k_psi_p = 0.0  # Gain on pressure error (A/psi)
        msg.k_psi_i = 0.0  # Gain on the integral of the pressure error (A/(psi/s)
        msg.k_psi_d = 0.0  # Gain on the derivative of the pressure error (A/(psi s)

        msg.k_rpm_p = 0.0  # Gain on rpm error (A / rpm)
        msg.k_rpm_i = 0.0  # Gain on the integral of the rpm error (A / (rpm s))
        msg.k_rpm_d = 0.0  # Gain on the derivative of the rpm error (A / (rpm/s)

        msg.ff_rpm_d = 0.0  # Feed-forward gain on the desired rpm (A / rpm)
        msg.ff_psi_d = 0.0  # Feed-forward gain on the desired pressure (A / psi)
        msg.ff_const = 0.0  # Constant current term (Amps)

        msg.psi_i_max = 0.0  # Max. abs. value to which the integral psi error is clamped (psi s)
        msg.rpm_i_max = 0.0  # Max. abs. value to which the integral rpm error is clamped (rpm s)

        # Max. command output (A). Default is 60 Amps.
        # This value may need to be lower than the default in order to avoid
        # causing the motor driver to fault.
        msg.cmd_max = 60

        msg.desired_psi = desiredPsi  # default should be 1500
        msg.desired_rpm = 5000  # default should be 5000

        lcmUtils.publish('ATLAS_PUMP_COMMAND', msg)
 def onStartNewMapButton(self):
     '''
     Send a trigger to the Create Octomap process to start a new map
     '''
     msg = lcmdrc.utime_t()
     msg.utime = getUtime()
     lcmUtils.publish('STATE_EST_START_NEW_MAP', msg)
Exemple #14
0
    def getSupportLCMFromListOfSupports(self, supportsList,ts):
        supports = [0]*len(ts)
        for i in xrange(len(ts)):
            supportElement = lcmdrc.support_element_t()
            supportElement.utime = getUtime()
            numBodies = len(supportsList[i])
            supportElement.num_bodies = numBodies
            supportBodies = []
            for j in xrange(numBodies):
                name = supportsList[i][j]
                if name is 'l_foot':
                    supportBodies.append(self.getFootSupportBodyMsg('left'))
                elif name is 'r_foot':
                    supportBodies.append(self.getFootSupportBodyMsg('right'))
                elif name is 'l_hand':
                    supportBodies.append(self.getHandSupportBodyMsg('left'))
                elif name is 'r_hand':
                    supportBodies.append(self.getHandSupportBodyMsg('right'))
                elif name is 'pelvis':
                    supportBodies.append(self.getPelvisSupportBodyMsg())
                else:
                    print "passed a support that isn't allowed"

            supportElement.support_bodies = supportBodies
            supports[i] = supportElement

        return supports
 def onStartNewMapButton(self):
     '''
     Send a trigger to the Create Octomap process to start a new map
     '''
     msg = lcmdrc.utime_t()
     msg.utime = getUtime()
     lcmUtils.publish('STATE_EST_START_NEW_MAP', msg)
 def onUseNewMapButton(self):
     '''
     Send a trigger to the GPF to use the newly created map
     '''
     msg = lcmdrc.utime_t()
     msg.utime = getUtime()
     lcmUtils.publish('STATE_EST_USE_NEW_MAP', msg)
Exemple #17
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)
Exemple #18
0
 def getSupports(self):
     if self.publishPlansWithSupports:
         supportElement = lcmdrc.support_element_t()
         supportElement.utime = getUtime()
         numBodies = 0
         supportBodies = []
         if self.pelvisSupportEnabled:
             numBodies += 1
             supportBodies.append(self.getPelvisSupportBodyMsg())
         if self.leftFootSupportEnabled:
             numBodies += 1
             supportBodies.append(self.getFootSupportBodyMsg('left'))
         if self.rightFootSupportEnabled:
             numBodies += 1
             supportBodies.append(self.getFootSupportBodyMsg('right'))
         if self.leftHandSupportEnabled:
             #numBodies += 2
             numBodies += 1
             supportBodies.append(self.getHandSupportBodyMsg('left', [0, 0, 1, 0]))
             #supportBodies.append(self.getHandSupportBodyMsg('left', [0, 0, -1, 0]))
         if self.rightHandSupportEnabled:
             #numBodies += 2
             numBodies += 1
             supportBodies.append(self.getHandSupportBodyMsg('right', [0, 0, 1, 0]))
             #supportBodies.append(self.getHandSupportBodyMsg('right', [0, 0, -1, 0]))
         supportElement.num_bodies = numBodies
         supportElement.support_bodies = supportBodies
         return [supportElement]
 def onUseNewMapButton(self):
     '''
     Send a trigger to the GPF to use the newly created map
     '''
     msg = lcmdrc.utime_t()
     msg.utime = getUtime()
     lcmUtils.publish('STATE_EST_USE_NEW_MAP', msg)
Exemple #20
0
    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)
Exemple #21
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)
Exemple #22
0
    def sendFingerControl(self, positionA, positionB, positionC, force, velocity, scissor, mode):
        assert 0.0 <= positionA <= 254.0
        assert 0.0 <= positionB <= 254.0
        assert 0.0 <= positionC <= 254.0
        assert 0.0 <= force <= 100.0
        assert 0.0 <= velocity <= 100.0
        assert 0 <= int(mode) <= 4

        if not scissor is None:
            assert 0.0 <= scissor <= 254.0

        msg = lcmrobotiq.command_t()
        msg.utime = getUtime()
        msg.activate = 1
        msg.emergency_release = 0
        msg.do_move = 1
        msg.ifc = 1
        msg.positionA = int(positionA)
        msg.positionB = int(positionB)
        msg.positionC = int(positionC)
        msg.force = int(254 * (force/100.0))
        msg.velocity = int(254 * (velocity/100.0))
        msg.mode = int(mode)

        if not scissor is None:
            msg.isc = 1
            msg.positionS = int(scissor)

        channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper()
        lcmUtils.publish(channel, msg)
Exemple #23
0
    def publish(self):
        messages = self.reader.getMessages()

        for message in messages:
            channel = message[2]
            val = message[3] / 127.0
            if channel is 102:
                self.msg.slider1 = val
            elif channel is 103:
                self.msg.slider2 = val
            elif channel is 104:
                self.msg.slider3 = val
            elif channel is 105:
                self.msg.slider4 = val
            elif channel is 106:
                self.msg.knob1 = val
            elif channel is 107:
                self.msg.knob2 = val
            elif channel is 108:
                self.msg.knob3 = val
            elif channel is 109:
                self.msg.knob4 = val
            elif channel is 110:
                self.msg.knob5 = val
            elif channel is 111:
                self.msg.knob6 = val
            elif channel is 112:
                self.msg.knob7 = val
            elif channel is 113:
                self.msg.knob8 = val

        if len(messages) is not 0:
            self.msg.utime = getUtime()
            lcmUtils.publish(self.lcmChannel, self.msg)
 def applyAllButtonClicked(self, event):
     msg = data_request_list_t()
     msg.utime = getUtime()
     msg.requests = []
     for name in self.groups:
         msg.requests.append(self.groups[name].getMessage())
     msg.num_requests = len(msg.requests)
     lcmUtils.publish('DATA_REQUEST', msg)
 def applyAllButtonClicked(self,event):
     msg = data_request_list_t()
     msg.utime = getUtime()
     msg.requests = []
     for name in self.groups:
         msg.requests.append(self.groups[name].getMessage())
     msg.num_requests = len(msg.requests)
     lcmUtils.publish('DATA_REQUEST',msg)
 def immediateRequestClicked(self, event):
     req = self.getMessage()
     req.period = 0
     msg = data_request_list_t()
     msg.utime = getUtime()
     msg.requests = [req]
     msg.num_requests = len(msg.requests)
     lcmUtils.publish('DATA_REQUEST', msg)
 def immediateRequestClicked(self,event):
     req = self.getMessage()
     req.period = 0
     msg = data_request_list_t()
     msg.utime = getUtime()
     msg.requests = [req]
     msg.num_requests = len(msg.requests)
     lcmUtils.publish('DATA_REQUEST',msg)
Exemple #28
0
    def sendClose(self, percentage=100.0):
        assert 0.0 <= percentage <= 100.0
        msg = lcmirobot.position_control_close_t()
        msg.utime = getUtime()
        msg.valid = [True, True, True, False]
        msg.close_fraction = percentage / 100.0

        channel = 'IROBOT_%s_POSITION_CONTROL_CLOSE' % self.side.upper()
        lcmUtils.publish(channel, msg)
Exemple #29
0
    def sendClose(self, percentage=100.0):
        assert 0.0 <= percentage <= 100.0
        msg = lcmirobot.position_control_close_t()
        msg.utime = getUtime()
        msg.valid = [True, True, True, False]
        msg.close_fraction = percentage / 100.0

        channel = 'IROBOT_%s_POSITION_CONTROL_CLOSE' % self.side.upper()
        lcmUtils.publish(channel, msg)
Exemple #30
0
    def setLidarRpm(rpm):

        m = lcmmultisense.command_t()
        m.utime = getUtime()
        m.fps = -1
        m.gain = -1
        m.agc = -1
        m.rpm = rpm

        lcmUtils.publish('MULTISENSE_COMMAND', m)
Exemple #31
0
 def getPelvisSupportBodyMsg(self):
     linkname = 'pelvis'
     supportBody = lcmdrc.support_body_t()
     supportBody.utime = getUtime()
     supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
     supportBody.contact_pts = np.array(self.ikPlanner.ikServer.comm.getFloatArray('pelvis_pts(:)')).reshape((3,-1), order='F')
     supportBody.num_contact_pts = len(supportBody.contact_pts[0])
     supportBody.use_support_surface = False
     supportBody.override_contact_pts = True
     return supportBody
Exemple #32
0
 def getFootSupportBodyMsg(self, side):
     linkname = 'l_foot' if side == 'left' else 'r_foot'
     supportBody = lcmdrc.support_body_t()
     supportBody.utime = getUtime()
     supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
     supportBody.contact_pts = np.array(self.ikPlanner.ikServer.comm.getFloatArray('%s_pts(:)' % linkname)).reshape((3,-1), order='F')
     supportBody.num_contact_pts = len(supportBody.contact_pts[0])
     supportBody.use_support_surface = False
     supportBody.override_contact_pts = True
     return supportBody
Exemple #33
0
 def _newCommandMessage(self, commandName, **commandArgs):
     commandId = newUUID()
     self.sentCommands.add(commandId)
     commandArgs['commandId'] = commandId
     commandArgs['collectionId'] = self.collectionId
     commandArgs['command'] = commandName
     msg = lcmdrc.affordance_collection_t()
     msg.name = numpyjsoncoder.encode(commandArgs)
     msg.utime = getUtime()
     return msg
Exemple #34
0
 def getHandSupportBodyMsg(self, side, support_surface):
     linkname = 'l_hand' if side == 'left' else 'r_hand'
     supportBody = lcmdrc.support_body_t()
     supportBody.utime = getUtime()
     supportBody.body_id = int(self.ikPlanner.ikServer.comm.getFloatArray('links.%s' % linkname)[0])
     supportBody.contact_pts = self.ikPlanner.getPalmPoint(side=side).reshape(3,1)
     supportBody.num_contact_pts = 1
     supportBody.use_support_surface = True
     supportBody.override_contact_pts = True
     supportBody.support_surface = support_surface
     return supportBody
Exemple #35
0
    def sendDataRequest(requestType, repeatTime=0.0):

      msg = lcmdrc.data_request_t()
      msg.type = requestType
      msg.period = int(repeatTime*10) # period is specified in tenths of a second

      msgList = lcmdrc.data_request_list_t()
      msgList.utime = getUtime()
      msgList.requests = [msg]
      msgList.num_requests = len(msgList.requests)
      lcmUtils.publish('DATA_REQUEST', msgList)
Exemple #36
0
 def sendDrop(self):
     msg = lcmrobotiq.command_t()
     msg.utime = getUtime()
     msg.activate = 1
     msg.emergency_release = 1
     msg.do_move = 0
     msg.mode = 0
     msg.position = 0
     msg.force = 0
     msg.velocity = 0
     channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper()
     lcmUtils.publish(channel, msg)
Exemple #37
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 = [transformUtils.positionMessageFromFrame(tform)]
     msg.region_id = [uid]
     msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
     lcmUtils.publish(self.request_channel, msg)
Exemple #38
0
    def setLidarRevolutionTime(secondsPerRevolution):

        assert secondsPerRevolution >= 1.0

        m = lcmmultisense.command_t()
        m.utime = getUtime()
        m.fps = -1
        m.gain = -1
        m.agc = -1
        m.rpm = 60.0 / (secondsPerRevolution)

        lcmUtils.publish('MULTISENSE_COMMAND', m)
Exemple #39
0
 def sendDrop(self):
     msg = lcmrobotiq.command_t()
     msg.utime = getUtime()
     msg.activate = 1
     msg.emergency_release = 1
     msg.do_move = 0
     msg.mode = 0
     msg.position = 0
     msg.force = 0
     msg.velocity = 0
     channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper()
     lcmUtils.publish(channel, msg)
Exemple #40
0
    def sendClose(self, percentage=100.0):
        assert 0.0 <= percentage <= 100.0

        msg = lcmrobotiq.command_t()
        msg.utime = getUtime()
        msg.activate = 1
        msg.emergency_release = 0
        msg.do_move = 1
        msg.position = int(254 * (percentage / 100.0))
        msg.force = 254
        msg.velocity = 254
        channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper()
        lcmUtils.publish(channel, msg)
Exemple #41
0
    def sendClose(self, percentage=100.0):
        assert 0.0 <= percentage <= 100.0

        msg = lcmrobotiq.command_t()
        msg.utime = getUtime()
        msg.activate = 1
        msg.emergency_release = 0
        msg.do_move = 1
        msg.position = int(254 * (percentage/100.0))
        msg.force = 254
        msg.velocity = 254
        channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper()
        lcmUtils.publish(channel, msg)
Exemple #42
0
    def sendMultisenseCommand(fps, gain, exposure, agc, rpm, led_flash, led_duty):

        m = lcmmultisense.command_t()
        m.utime = getUtime()
        m.fps = fps
        m.gain = gain
        m.exposure_us = exposure
        m.agc = agc
        m.rpm = rpm
        m.leds_flash = led_flash
        m.leds_duty_cycle = led_duty

        lcmUtils.publish('MULTISENSE_COMMAND', m)
Exemple #43
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 = [transformUtils.positionMessageFromFrame(tform)]
     msg.region_id = [uid]
     msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
     lcmUtils.publish(self.request_channel, msg)
Exemple #44
0
    def onStartMappingButton(self):
        msg = map_command_t()
        msg.timestamp = getUtime()
        msg.command = 0
        lcmUtils.publish('KINECT_MAP_COMMAND', msg)

        utime = self.queue.getCurrentImageTime('KINECT_RGB')
        self.cameraToLocalInit = vtk.vtkTransform()
        self.queue.getTransform('KINECT_RGB', 'local', utime, self.cameraToLocalInit)
        vis.updateFrame(self.cameraToLocalInit, 'initial cam' )
        print "starting mapping", utime
        print self.cameraToLocalInit.GetPosition()
        print self.cameraToLocalInit.GetOrientation()
Exemple #45
0
    def publishConstraints(self, constraints, messageName='PLANNER_REQUEST'):
        poses = getPlanPoses(constraints, self.ikPlanner)

        #poseJsonStr = json.dumps(poses, indent=4)
        #constraintsJsonStr = encodeConstraints(constraints, indent=4)
        poseJsonStr = json.dumps(poses)
        constraintsJsonStr = encodeConstraints(constraints)

        msg = lcmdrc.planner_request_t()
        msg.utime = getUtime()
        msg.poses = poseJsonStr
        msg.constraints = constraintsJsonStr
        lcmUtils.publish(messageName, msg)
    def publishConstraints(self,constraints,messageName='PLANNER_REQUEST'):
        poses = getPlanPoses(constraints, self.ikPlanner)

        #poseJsonStr = json.dumps(poses, indent=4)
        #constraintsJsonStr = encodeConstraints(constraints, indent=4)
        poseJsonStr = json.dumps(poses)
        constraintsJsonStr = encodeConstraints(constraints)

        msg = lcmdrc.planner_request_t()
        msg.utime = getUtime()
        msg.poses = poseJsonStr
        msg.constraints = constraintsJsonStr
        lcmUtils.publish(messageName, msg)
Exemple #47
0
    def setMode(self, mode):
        assert 0 <= int(mode) <= 4

        msg = lcmrobotiq.command_t()
        msg.utime = getUtime()
        msg.activate = 1
        msg.emergency_release = 0
        msg.do_move = 0
        msg.position = 0
        msg.force = 0
        msg.velocity = 0
        msg.mode = int(mode)
        channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper()
        lcmUtils.publish(channel, msg)
Exemple #48
0
    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 = transformUtils.positionMessageFromFrame(goalFrame)

        msg = self.applyParams(msg)
        msg = self.applySafeRegions(msg)
        return msg
Exemple #49
0
def drakePoseToQPInput(pose, atlasVersion=5):
    if atlasVersion == 4:
        numPositions = 34
    else:
        numPositions = 36

    msg = lcmt_qp_controller_input()
    msg.timestamp = getUtime()
    msg.num_support_data = 0
    msg.num_tracked_bodies = 0
    msg.num_external_wrenches = 0
    msg.num_joint_pd_overrides = 0
    msg.param_set_name = 'position_control'

    whole_body_data = lcmt_whole_body_data()
    whole_body_data.timestamp = getUtime()
    whole_body_data.num_positions = numPositions
    whole_body_data.q_des = pose
    whole_body_data.num_constrained_dofs = numPositions - 6
    whole_body_data.constrained_dofs = range(7, numPositions + 1)
    msg.whole_body_data = whole_body_data

    return msg