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
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