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)
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 sendFOVRequest(channel, imagePoints):
channelToImageType = {
'CAMERA_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_HEAD_LEFT,
'CAMERACHEST_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_LCHEST,
'CAMERACHEST_RIGHT' : lcmdrc.data_request_t.CAMERA_IMAGE_RCHEST,
}
dataRequest = lcmdrc.data_request_t()
dataRequest.type = channelToImageType[channel]
message = lcmdrc.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 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)
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 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 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 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 sendFOVRequest(channel, imagePoints):
channelToImageType = {
'CAMERA_LEFT': lcmdrc.data_request_t.CAMERA_IMAGE_HEAD_LEFT,
'CAMERACHEST_LEFT': lcmdrc.data_request_t.CAMERA_IMAGE_LCHEST,
'CAMERACHEST_RIGHT': lcmdrc.data_request_t.CAMERA_IMAGE_RCHEST,
}
dataRequest = lcmdrc.data_request_t()
dataRequest.type = channelToImageType[channel]
message = lcmdrc.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 processTraj(self, constraints, endPoseName="", nominalPoseName="", seedPoseName="", additionalTimeSamples=None):
# 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()
msg = self.setupMessage(constraints, endPoseName, nominalPoseName, seedPoseName, additionalTimeSamples)
lcmUtils.publish('PLANNER_REQUEST', msg)
lastManipPlan = listener.waitForResponse(timeout=20000)
listener.finish()
if lastManipPlan:
# HACK: need to multiply by original trajectory length again (the one in the plan is planning and not real time), otherwise jumps to real hardware
for state in lastManipPlan.plan:
state.utime = state.utime * 50
self.ikPlanner.ikServer.infoFunc(lastManipPlan.plan_info[0])
return lastManipPlan, lastManipPlan.plan_info[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)
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 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 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 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 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 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 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 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 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 onFootstepPlanContinuous(self, msg):
if msg.num_steps <= 2:
return
if self.navigationPanel.automaticContinuousWalkingEnabled:
print "Committing Footstep Plan for AUTOMATIC EXECUTION"
lcmUtils.publish('COMMITTED_FOOTSTEP_PLAN', msg)
def onFootstepPlanContinuous(self, msg):
if msg.num_steps <= 2:
return
if self.navigationPanel.automaticContinuousWalkingEnabled:
print "Committing Footstep Plan for AUTOMATIC EXECUTION"
lcmUtils.publish('COMMITTED_FOOTSTEP_PLAN', 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)
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 loadSDFFileAndRunSim(self):
from ddapp import sceneloader
filename= os.environ['DRC_BASE'] + '/../drc-testing-data/terrain/terrain_simple.sdf'
sc=sceneloader.SceneLoader()
sc.loadSDF(filename)
import ipab
msg=ipab.scs_api_command_t()
msg.command="loadSDF "+filename+"\nsimulate"
lcmUtils.publish('SCS_API_CONTROL', msg)
def apply3DFit(self):
if om.findObjectByName('drill') is None:
self.log('No 3D fit of drill. Click Spawn Drill button to provide a fit.')
msg = lcmdrc.pfgrasp_command_t()
msg.command = lcmdrc.pfgrasp_command_t.RUN_ONE_ITER_W_3D_PRIOR
affordanceReach = om.findObjectByName('grasp frame')
affordanceReach.actor.GetUserTransform().GetPosition(msg.pos)
lcmUtils.publish('PFGRASP_CMD', msg)
def updateDescription(self, desc, publish=True, notify=True):
self.collection[self.getDescriptionId(desc)] = desc
self._modified()
if publish:
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 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 _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 = 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)
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 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 = [transformUtils.positionMessageFromFrame(tform)]
msg.region_id = [uid]
msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
lcmUtils.publish(self.request_channel, msg)
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 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 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 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)
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 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 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)
def removeDescription(self, descriptionId, publish=True, notify=True):
try:
del self.collection[descriptionId]
self._modified()
except KeyError:
pass
if publish:
msg = self._newCommandMessage('remove', descriptionId=descriptionId,)
lcmUtils.publish(self.channel, msg)
if notify:
self.callbacks.process(self.DESCRIPTION_REMOVED_SIGNAL, self, descriptionId)
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 start(self, autoMode):
# start pfgrasp c++ program
startPfgraspHere = False
# initialize pfgrasp particles
msg = lcmdrc.pfgrasp_command_t()
msg.command = lcmdrc.pfgrasp_command_t.START
lcmUtils.publish('PFGRASP_CMD', msg)
self.autoMode = autoMode
if self.autoMode:
self.runoneiter()
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 sendFootstepPlanCheckRequest(self, request, waitForResponse=False, waitTimeout=5000):
assert isinstance(request, lcmdrc.footstep_check_request_t)
requestChannel = 'FOOTSTEP_CHECK_REQUEST'
responseChannel = 'FOOTSTEP_PLAN_RESPONSE'
if waitForResponse:
if waitTimeout == 0:
helper = lcmUtils.MessageResponseHelper(responseChannel, lcmdrc.footstep_plan_t)
lcmUtils.publish(requestChannel, request)
return helper
return lcmUtils.MessageResponseHelper.publishAndWait(requestChannel, request,
responseChannel, lcmdrc.footstep_plan_t, waitTimeout)
else:
lcmUtils.publish(requestChannel, request)
