def __init__( self , R = HM.HybridTwoLinkManipulator() ):
""" """
RCTC.RminSlidingModeController.__init__( self , R )
self.FixCtl = RCTC.RfixSlidingModeController( R )
# Assign Rollout Controller to Simulation Model
self.R.ctl = self.FixCtl.ctl
# Rollout Params
self.horizon = 1
self.sim_dt = 0.02
self.sim_n = self.horizon / self.sim_dt + 1
# Domain check in simulation?
self.domain_check = True
self.domain_fail_cost = 1
#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]]) #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]]) #RRT.U = np.array([[0,T,0],[0,0,0],[0,-T,0]]) RRT.dt = 0.2 RRT.goal_radius = 1.0 RRT.max_nodes = 12000 RRT.max_solution_time = 12 #RRT.compute_steps(1000,True) RRT.find_path_to_goal(x_goal) # Assign controller # Assign controller CTC_controller = RminCTC.RminSlidingModeController(R) CTC_controller.load_trajectory(RRT.solution) R.ctl = CTC_controller.ctl CTC_controller.lam = 1.0 CTC_controller.D = 10.0 CTC_controller.traj_ref_pts = 'closest' #CTC_controller.traj_ref_pts = 'interpol' CTC_controller.hysteresis = True CTC_controller.min_delay = 0.5 # Plot tf = RRT.time_to_goal + 5 n = int(np.round(tf / 0.05)) + 1 R.plotAnimation(x_start, tf, n, solver='euler')
else:
RRT.load_solution(name_traj)
#RRT.plot_open_loop_solution()
#RRT.plot_open_loop_solution_acc()
#RRT.solution_smoothing()
#RRT.plot_open_loop_solution()
#RRT.plot_open_loop_solution_acc()
if simulation:
# Assign controller
CTC_controller = RminCTC.RminComputedTorqueController(R)
CTC_controller.load_trajectory(RRT.solution)
CTC_controller.goal = x_goal
R.ctl = CTC_controller.ctl
CTC_controller.n_gears = 2
CTC_controller.w0 = 2.0
CTC_controller.zeta = 0.7
#CTC_controller.traj_ref_pts = 'closest'
CTC_controller.traj_ref_pts = 'interpol'
CTC_controller.hysteresis = True
CTC_controller.hys_level = 0.005
# Plot
tf = RRT.time_to_goal + 5
def load_params(self, event):
""" Load param on ROS server """
self.robot_type = rospy.get_param("robot_type" , 'pendulum' )
self.robot_config = rospy.get_param("robot_config", 'wrist-only' )
self.robot_ctl = rospy.get_param("controller", 'RfixCTC' )
self.fixed_mode = rospy.get_param("fixed_mode", 1 )
###############################################
# Load robot model for the right configuration
if self.robot_config == 'wrist-only':
self.R = Proto.SingleRevoluteDSDM()
elif self.robot_config == 'dual-plane' :
self.R = Proto.TwoPlanarSerialDSDM()
else:
self.R = None
###############################################
# Load controller
if self.robot_ctl == 'RfixCTC' :
self.Ctl = RminCTC.RfixComputedTorqueController( self.R , self.fixed_mode )
elif self.robot_ctl == 'RminCTC' :
self.Ctl = RminCTC.RminComputedTorqueController( self.R )
elif self.robot_ctl == 'RfixSLD' :
self.Ctl = RminCTC.RfixSlidingModeController( self.R , self.fixed_mode )
elif self.robot_ctl == 'RminSLD' :
self.Ctl = RminCTC.RminSlidingModeController( self.R )
elif self.robot_ctl == 'RollCTC' :
self.Ctl = RollCTC.RolloutComputedTorqueController( self.R )
elif self.robot_ctl == 'RollSLD' :
self.Ctl = RollCTC.RolloutSlidingModeController( self.R )
else:
self.Ctl = None
if self.robot_config == 'wrist-only':
self.Ctl.n_gears = rospy.get_param("n_gears", 2 )
self.x_d = np.array( rospy.get_param("goal", [0,0] ) )
elif self.robot_config == 'dual-plane' :
self.Ctl.n_gears = rospy.get_param("n_gears", 4 )
self.x_d = np.array( rospy.get_param("goal", [0.0,0.0,0.0,0.0] ) )
#self.x_d = np.array( [-3.14 , 0 , 0 , 0] )
# Gen ctl params
self.Ctl.hysteresis = rospy.get_param("hysteresis", True )
self.Ctl.min_delay = rospy.get_param("min_delay", 0.5 )
self.Ctl.w0 = rospy.get_param("w0", 1 )
self.Ctl.zeta = rospy.get_param("zeta", 0.7 )
self.Ctl.lam = rospy.get_param("lam", 1 )
self.Ctl.nab = rospy.get_param("nab", 1 )
self.Ctl.D = rospy.get_param("D", 0 )
self.Ctl.horizon = rospy.get_param("horizon", 0.5 )
self.Ctl.sim_dt = rospy.get_param("sim_dt", 0.1 )
self.Ctl.domain_check = rospy.get_param("domain_check", False )
# Base policy param for roll
if self.robot_ctl == 'RollCTC' :
self.Ctl.FixCtl.lam = self.Ctl.lam
elif self.robot_ctl == 'RollSLD' :
self.Ctl.FixCtl.lam = self.Ctl.lam
self.Ctl.FixCtl.nab = self.Ctl.nab
self.Ctl.FixCtl.D = self.Ctl.D
@author: alex """ 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.HybridOneLinkManipulator() # Assign controller Ctl = RminCTC.RminSlidingModeController( R ) #Ctl = RminCTC.RminComputedTorqueController( R ) #Ctl = RminCTC.RfixSlidingModeController( R , 1 ) R.ctl = Ctl.ctl Ctl.n_gears = 2 Ctl.w0 = 1.0 Ctl.lam = 1.0 Ctl.nab = 1.0 Ctl.D = 0 Ctl.hysteresis = True Ctl.min_delay = 0.5
#RRT.low_pass_filter.set_freq_to( fc = 5 , dt = RRT.dt )
if ReComputeTraj:
RRT.find_path_to_goal(x_goal)
RRT.plot_2D_Tree()
RRT.save_solution(name_traj)
print 'Solution Saved'
else:
RRT.load_solution(name_traj)
print 'Solution Loaded'
""" Assign controller """
CTC_controller = RminCTC.RfixComputedTorqueController(R_ctl)
CTC_controller.load_trajectory(RRT.solution)
CTC_controller.goal = x_goal
CTC_controller.w0 = 1.0
CTC_controller.zeta = 0.7
CTC_controller.R_index = 1
R.ctl = CTC_controller.ctl
""" Simulation """
tf = 5
R.computeSim(x_start, tf, n=int(10 / 0.001) + 1, solver='euler')
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