# -*- coding: utf-8 -*- """ Created on Sun May 12 16:34:44 2019 @author: Alexandre """ ############################################################################### import numpy as np ############################################################################### from pyro.planning import randomtree from pyro.dynamic import manipulator ############################################################################### torque_controlled_robot = manipulator.TwoLinkManipulatorwithObstacles() speed_controlled_robot = manipulator.SpeedControlledManipulator.from_manipulator( torque_controlled_robot ) q_start = np.array([0,0]) q_goal = np.array([1.57,0]) planner = randomtree.RRT( speed_controlled_robot , q_start ) planner.load_solution('twolinkplan.npy') planner.plot_tree() planner.plot_open_loop_solution() speed_controlled_robot.traj = planner.trajectory speed_controlled_robot.animate_simulation( time_factor_video = 50.0 )
import numpy as np ############################################################################### from pyro.dynamic import pendulum from pyro.control import nonlinear from pyro.planning import randomtree ############################################################################### sys = pendulum.DoublePendulum() ############################################################################### x_start = np.array([-3.14, 0, 0, 0]) x_goal = np.array([0, 0, 0, 0]) planner = randomtree.RRT(sys, x_start) t = 12 planner.u_options = [ np.array([-t, -t]), np.array([-t, +t]), np.array([+t, -t]), np.array([+t, +t]), np.array([0, +t]), np.array([0, -t]), np.array([0, 0]), np.array([+t, 0]), np.array([-t, 0]) ]
from pyro.dynamic import cartpole from pyro.planning import randomtree from pyro.planning import trajectoryoptimisation ############################################################################### sys = cartpole.UnderActuatedRotatingCartPole() sys.u_ub[0] = +50 # Max torque sys.u_lb[0] = -50 # Min torque ############################################################################### x_start = np.array([0, -3.14, 0, 0]) x_goal = np.array([0, 0, 0, 0]) rrt = randomtree.RRT(sys, x_start) rrt.u_options = [sys.u_ub, sys.u_lb] rrt.goal_radius = 1.5 rrt.dt = 0.1 rrt.max_nodes = 10000 rrt.max_solution_time = 3.0 rrt.max_distance_compute = 1000 rrt.dyna_plot = False rrt.find_path_to_goal(x_goal) ############################################################################### planner = trajectoryoptimisation.DirectCollocationTrajectoryOptimisation(