def load_traj( self, msg ):
""" Load Ref trajectory """
# Traj length ?
n = len( msg.points )
q = np.zeros([self.n_DOF,n])
dq = np.zeros([self.n_DOF,n])
ddq = np.zeros([self.n_DOF,n])
t = np.zeros(n)
# READ THE MESSAGE
#For all pts
for i in range(n):
# Time vector
secs = msg.points[i].time_from_start.secs
nsecs = msg.points[i].time_from_start.nsecs
t[i] = secs + nsecs * 0.001
# For all DOF
for j in range( self.n_DOF ):
q[j,i] = msg.points[i].positions[j]
dq[j,i] = msg.points[i].velocities[j]
ddq[j,i] = msg.points[i].accelerations[j]
###################
# Setup CTC class stuff
self.CTC.traj = [ ddq , dq , q , t ]
self.CTC.max_time = t.max()
# assign new controller
self.CTC.ctl = self.CTC.traj_following_ctl
# Create interpol functions
self.CTC.q = CTC.interp1d(t,q)
self.CTC.dq = CTC.interp1d(t,dq)
self.CTC.ddq = CTC.interp1d(t,ddq)
#####################
# Record new reference for time
self.t_zero = rospy.get_rostime()
self.trajectory_loaded = True
##################
if self.verbose:
rospy.loginfo("Controller: Ref Trajectory Loaded ")
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
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 R.plotAnimation(x0, tf, n, solver='euler')
""" 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]
########################### # Load libs ########################### from AlexRobotics.dynamic import Manipulator from AlexRobotics.planning import RandomTree from AlexRobotics.control import linear from AlexRobotics.control import ComputedTorque ########################### # Create objects ########################### Robot = Manipulator.OneLinkManipulator() CTC = ComputedTorque.ComputedTorqueController(Robot) RRT = RandomTree.RRT(Robot, x_start=[-3.0, 0.0]) PD = linear.PD(kp=5, kd=2) PID = linear.PID(kp=5, kd=2, ki=4) ########################### # Offline Plannning ########################### #RRT.find_path_to_goal( x_goal ) #RRT.plot_2D_Tree() ########################### # Assign controller ###########################