# -*- 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(
