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.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')
@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