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 = 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 = positionMessageFromFrame(t_stepbdi) if (self.showBDIPlan is True): self.drawBDIFootstepPlan()
def from_iris_region_t(msg): A, b = decodeLinCon(msg.lin_con) C = [] d = [] tform = frameFromPositionMessage(msg.seed_pose) return SafeTerrainRegion(A, b, C, d, tform)
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 = 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)