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
from AlexRobotics.dynamic import Prototypes as Proto from AlexRobotics.control import RminComputedTorque as RminCTC from AlexRobotics.dynamic import DynamicSystem as DS import numpy as np import matplotlib import matplotlib.pyplot as plt """ Modes """ save_fig = False all_fig = 'output/1link_xu.pdf' ReComputeTraj = False name_traj = 'data/pendulum_traj.npy' """ Define dynamic system """ R = Proto.SingleRevoluteDSDM() R.ubar = np.array([0.0, 474]) 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])
def __init__(self):
self.verbose = False
self.plot = True
# Message outgoing
self.pub_x = rospy.Publisher("x_hat", Float64MultiArray, queue_size=1)
# Messages Inputs
self.sub_a0 = rospy.Subscriber("a0/y",
dsdm_actuator_sensor_feedback,
self.feedback_callback_a0,
queue_size=1)
self.sub_a1 = rospy.Subscriber("a1/y",
dsdm_actuator_sensor_feedback,
self.feedback_callback_a1,
queue_size=1)
self.sub_a2 = rospy.Subscriber("a2/y",
dsdm_actuator_sensor_feedback,
self.feedback_callback_a2,
queue_size=1)
# Param Timer
self.timer = rospy.Timer(rospy.Duration.from_sec(2.0),
self.load_params)
# Load params
self.load_params(None)
# Load robot params
if self.robot_type == 'BoeingArm':
self.R = CM.BoeingArm()
self.plot_partial_config = True
elif self.robot_type == 'pendulum':
#self.R = CM.TestPendulum()
#self.plot_partial_config = False
self.R = CM.TestPendulum()
self.plot_partial_config = True
else:
print('Error loading robot type')
self.plot_partial_config = False
# Variable Init
self.a = np.zeros(self.R.dof)
self.da = np.zeros(self.R.dof)
self.q = np.zeros(self.R.dof)
self.dq = np.zeros(self.R.dof)
self.x_hat = self.R.q2x(self.q, self.dq)
if self.plot:
self.R.show_3D(self.q)
# Partial Manipulator
if self.plot_partial_config:
if self.robot_config == 'wrist-only':
self.Rp = Proto.SingleRevoluteDSDM()
self.qp = np.array([0])
elif self.robot_config == 'dual-plane':
self.Rp = Proto.TwoPlanarSerialDSDM()
self.qp = np.array([0, 0])
self.Rp.show(self.qp)
# -*- coding: utf-8 -*- """ Created on Sun Mar 6 15:27:04 2016 @author: alex """ from AlexRobotics.dynamic import Prototypes as Proto from AlexRobotics.control import RminComputedTorque as RminCTC from AlexRobotics.control import RolloutComputedTorque as RollCTC import numpy as np R_ctl = Proto.TwoPlanarSerialDSDM() R = Proto.TwoPlanarSerialDSDM() R.m2 = R_ctl.m2 + 0.3 # Assign controller Ctl = RollCTC.RolloutSlidingModeController(R_ctl) #Ctl = RminCTC.RminComputedTorqueController( R_ctl ) R.ctl = Ctl.ctl Ctl.n_gears = 4 Ctl.w0 = 8.0 Ctl.lam = 1.0 Ctl.nab = 1.0 Ctl.D = 1.0 Ctl.hysteresis = True Ctl.min_delay = 0.5