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
########################### 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 RRT.max_nodes = 5000 RRT.max_solution_time = 5
from AlexRobotics.dynamic import Hybrid_Manipulator as HM
from AlexRobotics.planning import RandomTree as RPRT
from AlexRobotics.control import RminComputedTorque as RminCTC
import numpy as np
import matplotlib.pyplot as plt
R = HM.HybridTwoLinkManipulator()
R.ubar = np.array([0, 0, 3])
x_start = np.array([3, 0, 0, 0])
x_goal = np.array([0, 0, 0, 0])
RRT = RPRT.RRT(R, x_start)
T = 5
#
RRT.U = np.array([[T, 0, 0], [0, 0, 0], [-T, 0, 0], [0, T, 0], [0, -T, 0],
[T, T, 0], [-T, -T, 0], [-T, T, 0], [T, -T, 0], [T, 0, 1],
[0, 0, 1], [-T, 0, 1], [0, T, 1], [0, -T, 1], [T, T, 1],
[-T, -T, 1], [-T, T, 1], [T, -T, 1], [T, 0, 2], [0, 0, 2],
[-T, 0, 2], [0, T, 2], [0, -T, 2], [T, T, 2], [-T, -T, 2],
[-T, T, 2], [T, -T, 2], [T, 0, 3], [0, 0, 3], [-T, 0, 3],
[0, T, 3], [0, -T, 3], [T, T, 3], [-T, -T, 3], [-T, T, 3],
[T, -T, 3]])
#RRT.U = np.array([[T,0,1],[0,0,1],[-T,0,1],[0,T,1],[0,-T,1],[T,T,1],[-T,-T,1],[-T,T,1],[T,-T,1]])
#RRT.U = np.array([[T,0,2],[0,0,2],[-T,0,2],[0,T,2],[0,-T,2],[T,T,2],[-T,-T,2],[-T,T,2],[T,-T,2]])
#RRT.U = np.array([[T,0,3],[0,0,3],[-T,0,3],[0,T,3],[0,-T,3],[T,T,3],[-T,-T,3],[-T,T,3],[T,-T,3]])
R.M = 1
R.ext_cst_force = 0
""" Define control model """
R_ctl = Proto.SingleRevoluteDSDM()
R_ctl.ubar = np.array([0.0, 474])
R_ctl.M = 1
R_ctl.ext_cst_force = 0
""" Define control problem """
x_start = np.array([-3.0, 0])
x_goal = np.array([0, 0])
""" Trajectory Planning """
RRT = RPRT.RRT(R_ctl, x_start)
T = 0.01
u_R1 = R_ctl.R[0]
u_R2 = R_ctl.R[1]
RRT.U = np.array([[T, u_R1], [0, u_R1], [-T, u_R1], [T, u_R2], [0, u_R2],
[-T, u_R2]])
RRT.dt = 0.05
RRT.goal_radius = 0.2
RRT.alpha = 0.8
RRT.max_nodes = 8000
RRT.max_solution_time = 5
# Make sure no low-gear is used at high-speed by the planner
def __init__(self):
self.verbose = True
# Sub
self.sub_set = rospy.Subscriber("setpoint" , joint_position , self.update_setpoint , queue_size=1 )
self.sub_enable = rospy.Subscriber("enable" , Bool , self.update_enable , queue_size=1 )
self.sub_mode = rospy.Subscriber("ctl_mode" , Int32 , self.update_mode , queue_size=1 )
self.sub_state_feedback = rospy.Subscriber("x_hat" , Float64MultiArray , self.update_state , queue_size=1 )
# Pub
#self.pub_control = rospy.Publisher("u", dsdm_robot_control_inputs , queue_size=1 )
self.pub_control = rospy.Publisher("a2/u", dsdm_actuator_control_inputs , queue_size=1 ) # Temp for testing, only one actuator
#self.pub_control = rospy.Publisher("CTC_U", dsdm_actuator_control_inputs , queue_size=1 )
self.pub_e = rospy.Publisher("ctc_error", ctl_error , queue_size=1 )
# Timer
#self.timer = rospy.Timer( rospy.Duration.from_sec(0.01), self.callback )
# Load ROS params
self.load_params( None )
# Assign controller
if self.robot_type == 'BoeingArm':
self.R = CM.BoeingArm()
self.CTC = RminCTC.RminComputedTorqueController( self.R )
elif self.robot_type == 'pendulum':
self.R = CM.TestPendulum()
self.CTC = RminCTC.RminComputedTorqueController( self.R )
# Temp to load traj
self.RRT = RPRT.RRT( self.R , np.array( [0,0,0,0,0,0] ) )
self.RRT.load_solution( '/home/alex/ROS_WS/src/dsdm_robotics/dsdm_control/data/pendulum_traj_test0.npy' )
else:
print('Error loading robot type')
# Load params
self.CTC.w0 = 8.0
self.CTC.zeta = 0.7
self.CTC.n_gears = 2 ##########
self.CTC.hysteresis = True
self.CTC.hys_level = 0.001
self.CTC.min_delay = 0.2
self.CTC.R.dq_max_HF = 1.3 # [rad/sec]
# Gear params
R_HS = self.CTC.R.R[0]
R_HF = self.CTC.R.R[1]
#self.CTC.last_gear_i = 0
#self.CTC.R.R = [ R_HS ] # only HF
self.R_HS = R_HS
self.R_HF = R_HF
# INIT
self.t_zero = rospy.get_rostime()
self.x = np.array([ 0 , 0 , 0 , 0 , 0 , 0])
self.ddq_d = np.array([ 0 , 0 , 0 ])
self.dq_d = np.array([ 0 , 0 , 0 ])
self.q_d = np.array([ 0 , 0 , 0 ])
self.enable = False
self.ctl_mode = 0
self.setpoint = 0
self.actual = 0
self.e = 0
self.de = 0
self.traj_started = False
# 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 ########################### #Robot.ctl = RRT.trajectory_controller