Esempio n. 1
0
    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()
Esempio n. 2
0
 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)
Esempio n. 3
0
    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)
Esempio n. 4
0
 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)