def sendFOVRequest(channel, imagePoints): import maps as lcmmaps channelToImageType = { "CAMERA_LEFT": lcmmaps.data_request_t.CAMERA_IMAGE_HEAD_LEFT, "CAMERACHEST_LEFT": lcmmaps.data_request_t.CAMERA_IMAGE_LCHEST, "CAMERACHEST_RIGHT": lcmmaps.data_request_t.CAMERA_IMAGE_RCHEST, } dataRequest = lcmmaps.data_request_t() dataRequest.type = channelToImageType[channel] message = lcmmaps.subimage_request_t() message.data_request = dataRequest imagePoints = np.array([[pt[0], pt[1]] for pt in imagePoints]) minX, maxX = imagePoints[:, 0].min(), imagePoints[:, 0].max() minY, maxY = imagePoints[:, 1].min(), imagePoints[:, 1].max() message.x = minX message.y = minY message.w = maxX - minX message.h = maxY - minY # print message.x, message.y, message.w, message.h requestChannel = "SUBIMAGE_REQUEST" lcmUtils.publish(requestChannel, message)
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 onIiwaStatus(self, msg): armJointPositions = list(msg.joint_position_measured) jointPosition = armJointPositions + self.fingerJointPositions m = lcmbotcore.robot_state_t() # m.utime = msg.utime # this used to get utimes from the kuka robot. should later fix in drake-iiwa-driver/src/kuka_driver.cc m.utime = getUtime() m.pose = robotstate.getPoseLCMFromXYZRPY(self.basePosition[0:3], self.basePosition[3:6]) m.twist = lcmbotcore.twist_t() m.twist.linear_velocity = lcmbotcore.vector_3d_t() m.twist.angular_velocity = lcmbotcore.vector_3d_t() m.num_joints = self.numJoints m.joint_name = self.jointNames m.joint_position = jointPosition m.joint_velocity = np.zeros(m.num_joints) m.joint_effort = np.zeros(m.num_joints) m.force_torque = lcmbotcore.force_torque_t() m.force_torque.l_hand_force = np.zeros(3) m.force_torque.l_hand_torque = np.zeros(3) m.force_torque.r_hand_force = np.zeros(3) m.force_torque.r_hand_torque = np.zeros(3) lcmUtils.publish('EST_ROBOT_STATE', m)
def planFootDownAndCenterWeight(self): leftFootDownPlan, leftFootDownEndPose = self.planLeftFootDown() centerWeightPlan = self.planCenterWeight(startPose=leftFootDownEndPose) # now we need to combine these plans footDownEndTime = leftFootDownPlan.plan.plan[-1].utime robotPlan = leftFootDownPlan robotPlan.plan.plan_info = list(robotPlan.plan.plan_info) for state, info in zip(centerWeightPlan.plan.plan, centerWeightPlan.plan.plan_info): state.utime += footDownEndTime robotPlan.plan.plan.append(state) robotPlan.plan.plan_info.append(info) robotPlan.plan.num_states = len(robotPlan.plan.plan) # make support sequence for support, t in zip(centerWeightPlan.support_sequence.supports, centerWeightPlan.support_sequence.ts): t += footDownEndTime robotPlan.support_sequence.ts.append(t) robotPlan.support_sequence.supports.append(support) robotPlan.is_quasistatic = True self.addPlan(robotPlan) lcmUtils.publish('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS', robotPlan) return robotPlan
def publishPlanWithSupports(self, keyFramePlan, supportsList, ts, isQuasistatic): manipPlanner = self.robotSystem.manipPlanner msg_robot_plan_t = manipPlanner.convertKeyframePlan(keyFramePlan) supports = manipPlanner.getSupportLCMFromListOfSupports(supportsList,ts) msg_robot_plan_with_supports_t = manipPlanner.convertPlanToPlanWithSupports(msg_robot_plan_t, supports, ts, isQuasistatic) lcmUtils.publish('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS', msg_robot_plan_with_supports_t) return msg_robot_plan_with_supports_t
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 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 processTraj(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName="", endPoseName=""): # Temporary fix / HACK / TODO (should be done in exotica_json) largestTspan = [0, 0] for constraintIndex, _ in enumerate(constraints): # Get tspan extend to normalise time-span if np.isfinite(constraints[constraintIndex].tspan[0]) and np.isfinite(constraints[constraintIndex].tspan[1]): largestTspan[0] = constraints[constraintIndex].tspan[0] if (constraints[constraintIndex].tspan[0] < largestTspan[0]) else largestTspan[0] largestTspan[1] = constraints[constraintIndex].tspan[1] if (constraints[constraintIndex].tspan[1] > largestTspan[1]) else largestTspan[1] # Temporary fix / HACK/ TODO to normalise time spans for constraintIndex, _ in enumerate(constraints): if np.isfinite(constraints[constraintIndex].tspan[0]) and np.isfinite(constraints[constraintIndex].tspan[1]): if largestTspan[1] != 0: constraints[constraintIndex].tspan[0] = constraints[constraintIndex].tspan[0] / largestTspan[1] constraints[constraintIndex].tspan[1] = constraints[constraintIndex].tspan[1] / largestTspan[1] listener = self.ikPlanner.getManipPlanListener() fields = self.setupFields(constraints, ikParameters, positionCosts, nominalPoseName, seedPoseName, endPoseName) msg = self.setupMessage(fields) lcmUtils.publish('PLANNER_REQUEST', msg) lastManipPlan = listener.waitForResponse(timeout=20000) listener.finish() self.ikPlanner.ikServer.infoFunc(lastManipPlan.plan_info[0]) return lastManipPlan, lastManipPlan.plan_info[0]
def publishTwoStepEstimateData(self, twoStepEstimateData, actualContactLocationsMsg): msg = cpf_lcmtypes.actual_and_estimated_contact_locations_t() msg.utime = self.getUtime() msg.actual_contact_location = actualContactLocationsMsg estMsg = cpf_lcmtypes.multiple_contact_location_t() estMsg.num_contacts = len(twoStepEstimateData) for linkName, data in twoStepEstimateData.iteritems(): tmpMsg = cpf_lcmtypes.single_contact_filter_estimate_t() tmpMsg.body_name = linkName tmpMsg.contact_normal = data['force'] if data['pt'] is None: tmpMsg.utime = -1 # signifies that no intersection was found else: tmpMsg.contact_position = data['contactLocation'] tmpMsg.contact_position_in_world = data[ 'contactLocationInWorld'] tmpMsg.contact_force = data['force'] tmpMsg.contact_force_in_world = data['forceInWorld'] estMsg.contacts.append(tmpMsg) msg.estimated_contact_location = estMsg lcmUtils.publish(self.options['twoStepEstimator']['publishChannel'], 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)
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 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 onIiwaStatus(msg): fingerHalfDist = lastGripperMsg.actual_position_mm * 1e-3 * 0.5 fingerJointNames = ['wsg_50_finger_left_joint', 'wsg_50_finger_right_joint'] fingerJointPositions = [fingerHalfDist, fingerHalfDist] armJointNames = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'] armJointPositions = list(msg.joint_position_measured) jointNames = armJointNames + fingerJointNames jointPositions = armJointPositions + fingerJointPositions m = lcmbotcore.robot_state_t() m.utime = msg.utime m.pose = robotstate.getPoseLCMFromXYZRPY([0,0,0], [0,0,0]) m.twist = lcmbotcore.twist_t() m.twist.linear_velocity = lcmbotcore.vector_3d_t() m.twist.angular_velocity = lcmbotcore.vector_3d_t() m.num_joints = len(jointNames) m.joint_name = jointNames m.joint_position = jointPositions m.joint_velocity = np.zeros(m.num_joints) m.joint_effort = np.zeros(m.num_joints) m.force_torque = lcmbotcore.force_torque_t() m.force_torque.l_hand_force = np.zeros(3) m.force_torque.l_hand_torque = np.zeros(3) m.force_torque.r_hand_force = np.zeros(3) m.force_torque.r_hand_torque = np.zeros(3) lcmUtils.publish('EST_ROBOT_STATE', m)
def sendElectricArmEnabledState(self, enabledState): msg = 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 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)
def sendFOVRequest(channel, imagePoints): import maps as lcmmaps channelToImageType = { 'CAMERA_LEFT': lcmmaps.data_request_t.CAMERA_IMAGE_HEAD_LEFT, 'CAMERACHEST_LEFT': lcmmaps.data_request_t.CAMERA_IMAGE_LCHEST, 'CAMERACHEST_RIGHT': lcmmaps.data_request_t.CAMERA_IMAGE_RCHEST, } dataRequest = lcmmaps.data_request_t() dataRequest.type = channelToImageType[channel] message = lcmmaps.subimage_request_t() message.data_request = dataRequest imagePoints = np.array([[pt[0], pt[1]] for pt in imagePoints]) minX, maxX = imagePoints[:, 0].min(), imagePoints[:, 0].max() minY, maxY = imagePoints[:, 1].min(), imagePoints[:, 1].max() message.x = minX message.y = minY message.w = maxX - minX message.h = maxY - minY #print message.x, message.y, message.w, message.h requestChannel = 'SUBIMAGE_REQUEST' lcmUtils.publish(requestChannel, message)
def sendEchoResponse(self, requestId=None): if requestId is None: requestId = newUUID() msg = self._newCommandMessage('echo_response', requestId=requestId, descriptions=self.collection) lcmUtils.publish(self.channel, msg)
def sendDesiredPumpPsi(self, desiredPsi): msg = 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 processIK(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName=""): fields = self.setupFields(constraints, ikParameters, positionCosts, nominalPoseName, seedPoseName) msg = self.setupMessage(fields) listener = self.ikPlanner.getManipIKListener() lcmUtils.publish('IK_REQUEST', msg) ikplan = listener.waitForResponse(timeout=12000) listener.finish() endPose = [0] * self.ikPlanner.jointController.numberOfJoints if ikplan.num_states > 0: endPose[len(endPose) - len(ikplan.plan[ikplan.num_states - 1].joint_position):] = ikplan.plan[ ikplan.num_states - 1].joint_position info = ikplan.plan_info[ikplan.num_states - 1] else: info = -1 self.ikPlanner.ikServer.infoFunc(info) return endPose, info
def commitManipPlan(self, manipPlan): for previousPlan in self.committedPlans: if previousPlan.utime == manipPlan.utime: raise Exception('Refusing to re-commit manipulation plan.') self.committedPlans.append(manipPlan) if isinstance(manipPlan, lcmdrc.robot_plan_w_keyframes_t): manipPlan = self.convertKeyframePlan(manipPlan) supports = self.getSupports() if supports is not None: manipPlan = self.convertPlanToPlanWithSupports( manipPlan, supports, [0.0], self.plansWithSupportsAreQuasistatic) manipPlan.utime = getUtime() channelMap = { lcmdrc.robot_plan_with_supports_t: 'COMMITTED_ROBOT_PLAN_WITH_SUPPORTS' } defaultChannel = 'COMMITTED_ROBOT_PLAN' channel = channelMap.get(type(manipPlan), defaultChannel) lcmUtils.publish(channel, manipPlan) self.callbacks.process(self.PLAN_COMMITTED, manipPlan)
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 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 onEstRobotState(self, msg): cameraToWorld = self.getCameraToWorld() # make the message and publish it out cameraToWorldMsg = lcmframe.rigidTransformMessageFromFrame( cameraToWorld) lcmUtils.publish(self.channelName, cameraToWorldMsg)
def processTraj(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName="", endPoseName=""): fields = self.setupFields(constraints, ikParameters, positionCosts, nominalPoseName, seedPoseName, endPoseName) fields = self.testEncodeDecode(fields) poses, poseTimes, info = self.ikServer.runIkTraj(fields) plan = self.makePlanMessage(poses, poseTimes, info, fields) lcmUtils.publish('CANDIDATE_MANIP_PLAN', plan) return plan, info
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 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)
def sendStatusMessage(self, timestamp, response): msg = lcmrl.viewer2_comms_t() msg.format = "viewer2_json" msg.format_version_major = 1 msg.format_version_minor = 0 data = dict(timestamp=timestamp, **response.toJson()) msg.data = json.dumps(data) msg.num_bytes = len(msg.data) lcmUtils.publish('DRAKE_VIEWER2_RESPONSE', msg)
def updateDescription(self, desc, publish=True, notify=True): self.collection[self.getDescriptionId(desc)] = desc self._modified() if publish and USE_LCM: msg = self._newCommandMessage('update', description=desc) lcmUtils.publish(self.channel, msg) if notify: self.callbacks.process(self.DESCRIPTION_UPDATED_SIGNAL, self, self.getDescriptionId(desc))
def sendControlMessage(contents): msg = lcmrl.viewer2_comms_t() msg.utime = getUtime() msg.format = 'rigid_body_sim_json' msg.format_version_major = 1 msg.format_version_minor = 1 data = dict(**contents) msg.data = bytearray(json.dumps(data), encoding='utf-8') msg.num_bytes = len(msg.data) lcmUtils.publish(controlChannel, msg)
def sendDataRequest(requestType, repeatTime=0.0): msg = lcmmaps.data_request_t() msg.type = requestType msg.period = int(repeatTime*10) # period is specified in tenths of a second msgList = lcmmaps.data_request_list_t() msgList.utime = getUtime() msgList.requests = [msg] msgList.num_requests = len(msgList.requests) lcmUtils.publish('DATA_REQUEST', msgList)
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 sendDataRequest(requestType, repeatTime=0.0): msg = lcmmaps.data_request_t() msg.type = requestType msg.period = int(repeatTime * 10) # period is specified in tenths of a second msgList = lcmmaps.data_request_list_t() msgList.utime = getUtime() msgList.requests = [msg] msgList.num_requests = len(msgList.requests) lcmUtils.publish('DATA_REQUEST', msgList)
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)
def publishCorrection(self, channel='POSE_YAW_LOCK'): pelvisToWorld = self.getPelvisEstimate() position, quat = transformUtils.poseFromTransform(pelvisToWorld) msg = lcmbot.pose_t() msg.utime = robotStateJointController.lastRobotStateMessage.utime msg.pos = [0.0, 0.0, 0.0] msg.orientation = quat.tolist() lcmUtils.publish(channel, 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 _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 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 sendStatusMessage(self, timestamp, response, client_id=""): msg = lcmrl.viewer2_comms_t() msg.format = "treeviewer_json" msg.format_version_major = 1 msg.format_version_minor = 0 data = dict(timestamp=timestamp, **response.toJson()) msg.data = json.dumps(data) msg.num_bytes = len(msg.data) if client_id: channel = "DIRECTOR_TREE_VIEWER_RESPONSE_<{:s}>".format(client_id) else: channel = "DIRECTOR_TREE_VIEWER_RESPONSE" lcmUtils.publish(channel, msg)
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)
def sendStatusMessage(self, timestamp, response, client_id=""): msg = lcmrl.viewer2_comms_t() msg.format = "treeviewer_json" msg.format_version_major = 1 msg.format_version_minor = 0 data = dict(timestamp=timestamp, **response.toJson()) msg.data = bytearray(json.dumps(data), encoding='utf-8') msg.num_bytes = len(msg.data) if client_id: channel = "DIRECTOR_TREE_VIEWER_RESPONSE_<{:s}>".format(client_id) else: channel = "DIRECTOR_TREE_VIEWER_RESPONSE" lcmUtils.publish(channel, msg)
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()
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)
def removeDescription(self, descriptionId, publish=True, notify=True): try: del self.collection[descriptionId] self._modified() except KeyError: pass if publish and USE_LCM: msg = self._newCommandMessage('remove', descriptionId=descriptionId,) lcmUtils.publish(self.channel, msg) if notify: self.callbacks.process(self.DESCRIPTION_REMOVED_SIGNAL, self, descriptionId)