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
########################### # 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 RRT.x_start = x_start RRT.discretizeactions(3) RRT.dt = 0.1 RRT.goal_radius = 0.3
""" 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 """ # Ploting a trajectory x0 = [0,1,1,0,0,0]