def __init__( self , R = HM.HybridTwoLinkManipulator() ):
""" """
RCTC.RminComputedTorqueController.__init__( self , R )
self.FixCtl = RCTC.RfixComputedTorqueController( R )
# Assign Rollout Controller to Simulation Model
self.R.ctl = self.FixCtl.ctl
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
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
#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')