    def execute(self, path, dynamics=False):
        '''
        Execute a planned plan. Will visualize in pybullet.
        Args:
            path: list[state], a list of state
            dynamics: allow dynamic simulation. If dynamics is false, this API will use robot.set_state(),
                      meaning that the simulator will simply reset robot's state WITHOUT any dynamics simulation. Since the
                      path is collision free, this is somewhat acceptable.
        '''
        for q in path:
            if dynamics:
                for i in range(self.robot.num_dim):
                    p.setJointMotorControl2(self.robot.id, i, p.POSITION_CONTROL, q[i],force=5 * 240.)
            else:
                self.robot.set_state(q)
            p.stepSimulation()
            time.sleep(0.01)
Execute a planned plan. Will visualize in pybullet.
Args:
path: list[state], a list of state
dynamics: allow dynamic simulation. If dynamics is false, this API will use robot.set_state(),
meaning that the simulator will simply reset robot's state WITHOUT any dynamics simulation. Since the
path is collision free, this is somewhat acceptable.
pybullet_ompl/pb_ompl.py