def transformPlanToBDIFrame(self, plan):
if (self.pose_bdi is None):
# print "haven't received POSE_BDI"
return
# TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame
# instead of using FK here
t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation)
t_bodybdi.PostMultiply()
current_pose = self.jointController.q
t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , transformUtils.rollPitchYawToQuaternion(current_pose[3:6]) )
t_bodymain.PostMultiply()
# iterate and transform
self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy
for i, footstep in enumerate(self.bdi_plan.footsteps):
step = footstep.pos
t_step = transformUtils.frameFromPositionMessage(step)
t_body_to_step = vtk.vtkTransform()
t_body_to_step.DeepCopy(t_step)
t_body_to_step.PostMultiply()
t_body_to_step.Concatenate(t_bodymain.GetLinearInverse())
t_stepbdi = vtk.vtkTransform()
t_stepbdi.DeepCopy(t_body_to_step)
t_stepbdi.PostMultiply()
t_stepbdi.Concatenate(t_bodybdi)
footstep.pos = transformUtils.positionMessageFromFrame(t_stepbdi)
if (self.showBDIPlan is True):
self.drawBDIFootstepPlan()
def transformPlanToBDIFrame(self, plan):
if (self.pose_bdi is None):
# print "haven't received POSE_BDI"
return
# TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame
# instead of using FK here
t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation)
t_bodybdi.PostMultiply()
current_pose = self.jointController.q
t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , botpy.roll_pitch_yaw_to_quat(current_pose[3:6]) )
t_bodymain.PostMultiply()
# iterate and transform
self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy
for i, footstep in enumerate(self.bdi_plan.footsteps):
step = footstep.pos
t_step = transformUtils.frameFromPositionMessage(step)
t_body_to_step = vtk.vtkTransform()
t_body_to_step.DeepCopy(t_step)
t_body_to_step.PostMultiply()
t_body_to_step.Concatenate(t_bodymain.GetLinearInverse())
t_stepbdi = vtk.vtkTransform()
t_stepbdi.DeepCopy(t_body_to_step)
t_stepbdi.PostMultiply()
t_stepbdi.Concatenate(t_bodybdi)
footstep.pos = transformUtils.positionMessageFromFrame(t_stepbdi)
if (self.showBDIPlan is True):
self.drawBDIFootstepPlan()
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
def from_iris_region_t(msg):
A, b = decodeLinCon(msg.lin_con)
C = []
d = []
tform = transformUtils.frameFromPositionMessage(msg.seed_pose)
return SafeTerrainRegion(A, b, C, d, tform)
def from_iris_region_t(msg):
A, b = decodeLinCon(msg.lin_con)
C = []
d = []
tform = transformUtils.frameFromPositionMessage(msg.seed_pose)
return SafeTerrainRegion(A, b, C, d, tform)