예제 #1
0
    def getPlanPoses(msgOrList):

        if isinstance(msgOrList, list):
            messages = msgOrList
            allPoseTimes, allPoses = PlanPlayback.getPlanPoses(messages[0])

            for msg in messages[1:]:
                poseTimes, poses = PlanPlayback.getPlanPoses(msg)
                poseTimes += allPoseTimes[-1]
                allPoseTimes = np.hstack((allPoseTimes, poseTimes[1:]))
                allPoses += poses[1:]
            return allPoseTimes, allPoses

        else:
            msg = robotstate.asRobotPlan(msgOrList)

            poses = []
            poseTimes = []
            for plan in msg.plan:
                pose = robotstate.convertStateMessageToDrakePose(plan)
                poseTimes.append(plan.utime / 1e6)
                poses.append(pose)
            return np.array(poseTimes), poses
예제 #2
0
    def getPlanPoses(msgOrList):

        if isinstance(msgOrList, list):
            messages = msgOrList
            allPoseTimes, allPoses = PlanPlayback.getPlanPoses(messages[0])

            for msg in messages[1:]:
                poseTimes, poses = PlanPlayback.getPlanPoses(msg)
                poseTimes += allPoseTimes[-1]
                allPoseTimes = np.hstack((allPoseTimes, poseTimes[1:]))
                allPoses += poses[1:]
            return allPoseTimes, allPoses

        else:
            msg = robotstate.asRobotPlan(msgOrList)

            poses = []
            poseTimes = []
            for plan in msg.plan:
                pose = robotstate.convertStateMessageToDrakePose(plan)
                poseTimes.append(plan.utime / 1e6)
                poses.append(pose)
            return np.array(poseTimes), poses
예제 #3
0
 def getPlanInfo(self, plan):
     plan = robotstate.asRobotPlan(self.plan)
     return max(plan.plan_info)
예제 #4
0
 def isPlanFeasible(self):
     plan = robotstate.asRobotPlan(self.plan)
     return plan is not None and (max(plan.plan_info) < 10
                                  and min(plan.plan_info) >= 0)
예제 #5
0
 def getPlanInfo(self, plan):
     plan = robotstate.asRobotPlan(self.plan)
     return max(plan.plan_info)
예제 #6
0
 def isPlanFeasible(self):
     plan = robotstate.asRobotPlan(self.plan)
     return plan is not None and (max(plan.plan_info) < 10 and min(plan.plan_info) >= 0)
예제 #7
0
 def getPlanElapsedTime(msg):
     msg = robotstate.asRobotPlan(msg)
     startTime = msg.plan[0].utime
     endTime = msg.plan[-1].utime
     return (endTime - startTime) / 1e6
예제 #8
0
 def getPlanElapsedTime(msg):
     msg = robotstate.asRobotPlan(msg)
     startTime = msg.plan[0].utime
     endTime = msg.plan[-1].utime
     return (endTime - startTime) / 1e6