def __init__(self, R=M.TwoLinkManipulator()):
""" """
self.R = R # Model of the robot used by the controller
# Params
self.w0 = 10
self.zeta = 0.7
# default is fixed goal at zero
self.goal = np.zeros(R.n)
self.traj_loaded = False
self.ctl = self.fixed_goal_ctl
# Or load a solution: self.solution = [ x , u , t , dx ]
#self.traj_ref_pts = 'closest'
self.traj_ref_pts = 'interpol'
# For manual acc control
self.ddq_manual_setpoint = np.zeros(self.R.dof)
# Integral action with dist observer (beta)
self.dist_obs_active = False
self.obs = OBS.DistObserver(R)
def __init__(self, R=M.TwoLinkManipulator()):
""" """
ComputedTorqueController.__init__(self, R)
# Params
self.lam = 1 # Sliding surface slope
self.D = 1 # Discontinuous gain
self.nab = 0.1 # Min convergence rate
def ctl( x , t ):
u = np.array([0,0])
################################
# Your controller here
R = Manipulator.TwoLinkManipulator()
# Define controller
ctc = CTC.ComputedTorqueController( R )
ctc.w0 = 1
u = ctc.ctl( x , t )
#################################
return u
def setup_RRT_algo(self):
""" Init algo with right params """
# Robot model
self.R = M.TwoLinkManipulator()
#RRT algo
self.RRT = RPRT.RRT(self.R, x_start=np.array([3, 0, 0, 0]))
T = 12 # torque
self.RRT.U = np.array([[T, 0], [0, 0], [-T, 0], [0, T], [0, -T],
[T, T], [-T, -T], [-T, T], [T, -T]])
#RRT.U = np.array([[0,T],[0,-T],[0,0]]) # Acrobot problem
self.RRT.dt = 0.1
self.RRT.goal_radius = 1.0
self.RRT.max_nodes = 15000
self.RRT.max_solution_time = 10
from AlexRobotics.control import ComputedTorque from AlexRobotics.planning import RandomTree from AlexRobotics.control import DPO ########################### # Objectives ########################### x_start = np.array([-3.0, 0.0]) x_goal = np.array([0.0, 0.0]) ########################### # Create objects ########################### Robot = Manipulator.OneLinkManipulator() PD = linear.PD(kp=5, kd=2) PID = linear.PID(kp=5, kd=2, ki=4) CTC = ComputedTorque.ComputedTorqueController(Robot) SLD = ComputedTorque.SlidingModeController(Robot) RRT = RandomTree.RRT(Robot, x_start) VI = DPO.ValueIteration1DOF(Robot, 'quadratic') ############################ # Params ############################ tmax = 8 # max motor torque Robot.u_ub = np.array([tmax]) # Control Upper Bounds Robot.u_lb = np.array([-tmax]) # Control Lower Bounds
# -*- coding: utf-8 -*- """ Created on Sat Mar 5 20:24:50 2016 @author: alex """ from AlexRobotics.dynamic import Manipulator as M from AlexRobotics.control import ComputedTorque as CTC import matplotlib.pyplot as plt """ Define system """ # Define dynamic system R = M.TwoLinkManipulator() # Define controller CTC_controller = CTC.SlidingModeController(R) CTC_controller.lam = 1 CTC_controller.nab = 1 CTC_controller.D = 0 # Asign feedback law to the dynamic system R.ctl = CTC_controller.ctl """ Simulation and plotting """ # Ploting a trajectory x0 = [3, -1, 0, 0] tf = 10 dt = 0.01 n = int(tf / dt) + 1
# -*- coding: utf-8 -*- """ Created on Sun Mar 6 15:27:04 2016 @author: alex """ from AlexRobotics.planning import RandomTree as RPRT from AlexRobotics.dynamic import Manipulator as M import numpy as np import matplotlib.pyplot as plt R = M.OneLinkManipulator() tmax = 10 R.u_ub = np.array([tmax]) # Control Upper Bounds R.u_lb = np.array([-tmax]) # Control Lower Bounds x_start = np.array([-3.0, 0]) x_goal = np.array([0, 0]) RRT = RPRT.RRT(R, x_start) RRT.dt = 0.1 RRT.goal_radius = 0.3 RRT.max_nodes = 5000 RRT.max_solution_time = 5 # Dynamic plot
Created on Sat Mar 5 20:24:50 2016 @author: alex """ from AlexRobotics.dynamic import Manipulator as M from AlexRobotics.control import ComputedTorque as CTC import matplotlib.pyplot as plt import numpy as np """ Define system """ # Define dynamic system R = M.ThreeLinkManipulator() # Define controller CTC_controller = CTC.ComputedTorqueController( R ) CTC_controller.w0 = 1 q_d = np.array([ np.pi, -np.pi * 0.25 , -np.pi * 0.25 ]) dq_d = np.zeros( R.dof ) x_d = R.q2x( q_d , dq_d) CTC_controller.goal = x_d # Asign feedback law to the dynamic system R.ctl = CTC_controller.ctl """ Simulation and plotting """
###########################
# Load 2 dof robot
###########################
x_start = [-1,0.3,0,0]
###########################
# test student controller
###########################
os.chdir('students/')
for file in os.listdir():
Robot = Manipulator.TwoLinkManipulator()
name, extension = os.path.splitext( file )
code2test = importlib.import_module(name)
# Asign sudent controller to simulator
Robot.ctl = code2test.ctl
# Simulation
Robot.plotAnimation( x_start , tf=5, n=10001, solver='euler')
Robot.fig.canvas.set_window_title(name)
print(name,' Integral Cost:', Robot.Sim.J, ' Note:', max([0,100-Robot.Sim.J*0.05]))
# -*- coding: utf-8 -*- """ Created on Sat Mar 5 20:24:50 2016 @author: alex """ from AlexRobotics.dynamic import Manipulator as M from AlexRobotics.control import ComputedTorque as CTC import matplotlib.pyplot as plt import numpy as np """ Define system """ # Define real dynamic system R = M.TwoLinkManipulator() R.f_dist_steady = np.array([10, 10]) # Define approx dynamic system used by controller R_hat = M.TwoLinkManipulator() #R_hat.f_dist_steady = np.array([ 0 , 0 ]) # Model not aware of disturbance # Define controller CTC_controller = CTC.ComputedTorqueController(R_hat) CTC_controller.w0 = 1 #CTC_controller.dist_obs_active = False CTC_controller.dist_obs_active = True # Asign feedback law to the dynamic system R.ctl = CTC_controller.ctl
