def drakePoseToQPInput(pose,
atlasVersion=5,
useValkyrie=True,
useConstrainedDofs=False,
fixedBase=False,
forceControl=False):
numFloatingBaseJoints = 6
# only publish the fixed base joints
if fixedBase:
numFloatingBaseJoints = 0
pose = pose[6:]
numPositions = np.size(pose)
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
if useValkyrie:
if forceControl:
msg.param_set_name = 'base'
else:
msg.param_set_name = 'streaming'
else:
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
# what are these? Is it still correct for valkyrie
# if useValkyrie:
# whole_body_data.num_constrained_dofs = len(valConstrainedJointsIdx)
# whole_body_data.constrained_dofs = valConstrainedJointsIdx
# else:
# whole_body_data.num_constrained_dofs = numPositions - 6
# whole_body_data.constrained_dofs = range(7, numPositions+1)
# be careful with off by one indexing of constrained_dofs for lcm
if useConstrainedDofs:
# if we aren't using the fixed base then remove the fixed base from this
whole_body_data.num_constrained_dofs = numPositions - numFloatingBaseJoints
whole_body_data.constrained_dofs = list(
range(numFloatingBaseJoints + 1, numPositions + 1))
msg.whole_body_data = whole_body_data
return msg
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
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
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
def drakePoseToQPInput(pose, atlasVersion=5, useValkyrie=True, useConstrainedDofs=False, fixedBase=False,
forceControl=False):
numFloatingBaseJoints = 6
# only publish the fixed base joints
if fixedBase:
numFloatingBaseJoints = 0
pose = pose[6:]
numPositions = np.size(pose)
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
if useValkyrie:
if forceControl:
msg.param_set_name = 'base'
else:
msg.param_set_name = 'streaming'
else:
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
# what are these? Is it still correct for valkyrie
# if useValkyrie:
# whole_body_data.num_constrained_dofs = len(valConstrainedJointsIdx)
# whole_body_data.constrained_dofs = valConstrainedJointsIdx
# else:
# whole_body_data.num_constrained_dofs = numPositions - 6
# whole_body_data.constrained_dofs = range(7, numPositions+1)
# be careful with off by one indexing of constrained_dofs for lcm
if useConstrainedDofs:
# if we aren't using the fixed base then remove the fixed base from this
whole_body_data.num_constrained_dofs = numPositions - numFloatingBaseJoints
whole_body_data.constrained_dofs = range(numFloatingBaseJoints+1, numPositions+1)
msg.whole_body_data = whole_body_data
return 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 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 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 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 setupFields(self,
constraints,
ikParameters,
positionCosts,
nominalPoseName="",
seedPoseName="",
endPoseName=""):
poses = ikconstraintencoder.getPlanPoses(constraints, self.ikPlanner)
poses.update(self.poses)
poses['q_nom'] = list(self.ikPlanner.jointController.getPose('q_nom'))
fields = FieldContainer(
utime=getUtime(),
poses=poses,
constraints=constraints,
seedPose=seedPoseName,
nominalPose=nominalPoseName,
endPose=endPoseName,
jointNames=self.jointNames,
jointLimits=self.jointLimits,
positionCosts=positionCosts,
affordances=self.processAffordances(),
options=ikParameters,
)
return fields
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 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 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 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 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)
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 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 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 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 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 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 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 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 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 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 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 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 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 _newCommandMessage(self, commandName, **commandArgs):
commandId = newUUID()
self.sentCommands.add(commandId)
commandArgs['commandId'] = commandId
commandArgs['collectionId'] = self.collectionId
commandArgs['command'] = commandName
msg = lcmbotcore.system_status_t()
msg.value = numpyjsoncoder.encode(commandArgs)
msg.utime = getUtime()
return msg
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
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
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 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
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
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)
def _newCommandMessage(self, commandName, **commandArgs):
commandId = newUUID()
self.sentCommands.add(commandId)
commandArgs['commandId'] = commandId
commandArgs['collectionId'] = self.collectionId
commandArgs['command'] = commandName
msg = lcmbotcore.system_status_t()
msg.value = numpyjsoncoder.encode(commandArgs)
msg.utime = getUtime()
return msg
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
def getUtime(self):
msg = self.robotSystem.robotStateJointController.lastRobotStateMessage
utime = 0
if msg is not None:
utime = msg.utime
else:
utime = utimeUtil.getUtime() / 5.0 # slow down time by factor of 5
return utime
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 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)
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
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
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 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)
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 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)
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 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 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 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)
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 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()
