""" from AlexRobotics.planning import RandomTree as RPRT from AlexRobotics.dynamic import CustomManipulator as CM from AlexRobotics.control import RminComputedTorque as RminCTC import numpy as np import matplotlib.pyplot as plt ReComputeTraj = False saving_auto = False simulation = True name_traj = '../data/pendulum_traj.npy' #################################### R = CM.TestPendulum() R.ubar = np.array([0, 0, 0, 1]) x_start = np.array([-np.pi, 0, 0, 0, 0, 0]) x_goal = np.array([0, 0, 0, 0, 0, 0]) RRT = RPRT.RRT(R, x_start) T = 0.02 u_R1 = 0 u_R2 = 1 #RRT.U = np.array([ [ T,0,0,u_R1] , [ 0,0,0,u_R1] , [ -T,0,0,u_R1] ]) RRT.U = np.array([[T, 0, 0, u_R1], [0, 0, 0, u_R1], [-T, 0, 0, u_R1],
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 Aug 14 13:27:30 2016 @author: agirard """ from AlexRobotics.dynamic import CustomManipulator as CM from AlexRobotics.control import RminComputedTorque as RminCTC import numpy as np R = CM.BoeingArm() R.mc0 = 1.0 x0 = np.array([2, 0, 0, 0, 0, 0]) # Assign controller CTC_controller = RminCTC.RminComputedTorqueController(R) R.ctl = CTC_controller.ctl CTC_controller.w0 = 0.5 CTC_controller.zeta = 0.7 CTC_controller.n_gears = 2 R.plot3DAnimation(x0) R.Sim.plot_CL('u')
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