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 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 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 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 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 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 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 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 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 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 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 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)
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)
