# Fill the candidate contact points and surfaces vectors for name in conf.contact_frames: contact_points += [ContactPoint(robot.model, robot.data, name)] for name in conf.contact_surface_name: contact_surfaces += [ContactSurface(conf.contact_surface_name, conf.contact_surface_pos, conf.contact_normal, conf.K, conf.B, conf.mu)] nq, nv = robot.nq, robot.nv n = nq+nv # state size m = robot.na # control sizecost_scale U = np.zeros((N,m)) # initial guess for control inputs ode = ODERobot('ode', robot) # create simulator simu = RobotSimulator(conf, robot) # Initialize the trajectory that we would like to track traj = TrajectoryCirc(conf.T) # traj = TrajectoryLin(conf.p0, conf.p_des, conf.T) traj.compute(0) p_0 = np.zeros(3) np.copyto(p_0, traj.pos) # evaluate inverse kinematics for q0 IK = InverseKinematic(robot, eps=1e-3, MAX_ITER=10000) M_0 = np.array([[1,0,0],[0,-1,0],[0,0,-1]]) # M_0 = np.array([[0,1,0],[1,0,0],[0,0,-1]]) q_0 = IK.compute(p_0, M_0, conf.q0) x0 = np.concatenate((q_0, np.zeros(6)))
from utils.robot_simulator import RobotSimulator import ex_2_conf as conf print("".center(conf.LINE_WIDTH,'#')) print(" Cartesian Space Control - Manipulator ".center(conf.LINE_WIDTH, '#')) print("".center(conf.LINE_WIDTH,'#'), '\n') PLOT_JOINT_POS = 0 PLOT_JOINT_VEL = 0 PLOT_JOINT_ACC = 0 PLOT_TORQUES = 1 PLOT_EE_POS = 1 r = loadUR() robot = RobotWrapper(r.model, r.collision_model, r.visual_model) simu = RobotSimulator(conf, robot) # get the ID corresponding to the frame we want to control assert(robot.model.existFrame(conf.frame_name)) frame_id = robot.model.getFrameId(conf.frame_name) nx, ndx = 3, 3 N = int(conf.T_SIMULATION/conf.dt) # number of time steps tau = np.empty((robot.na, N))*nan # joint torques tau_c = np.empty((robot.na, N))*nan # joint Coulomb torques q = np.empty((robot.nq, N+1))*nan # joint angles v = np.empty((robot.nv, N+1))*nan # joint velocities dv = np.empty((robot.nv, N+1))*nan # joint accelerations x = np.empty((nx, N))*nan # end-effector position dx = np.empty((ndx, N))*nan # end-effector velocity ddx = np.empty((ndx, N))*nan # end effector acceleration
tests += [{'controller': 'OSC', 'kp': 200, 'friction': 4}] tests += [{'controller': 'IC', 'kp': 200, 'friction': 4}] tests += [{'controller': 'OSC', 'kp': 400, 'friction': 4}] tests += [{'controller': 'IC', 'kp': 400, 'friction': 4}] # tests += [{'controller': 'OSC', 'kp': 1100, 'friction': 20}] # tests += [{'controller': 'IC', 'kp': 1100, 'friction': 20}] # tests += [{'controller': 'OSC', 'kp': 1400, 'friction': 20}] # tests += [{'controller': 'IC', 'kp': 1400, 'friction': 20}] # get the ID corresponding to the frame we want to control assert (robot.model.existFrame(conf.frame_name)) frame_id = robot.model.getFrameId(conf.frame_name) simu = RobotSimulator(conf, robot) tracking_err_osc = [] # list to contain the tracking error of OSC tracking_err_ic = [] # list to contain the tracking error of IC kp_j, kd_j = conf.kp_j, conf.kd_j lamb = 8 for (test_id, test) in enumerate(tests): description = str(test_id)+' Controller '+test['controller']+' kp='+\ str(test['kp'])+' friction='+str(test['friction']) print(description) kp = test['kp'] kd = 2 * np.sqrt(kp) tau_coulomb_max = test['friction'] * np.ones( 6) # expressed as percentage of torque max simu.set_coulomb_friction(
# horizon size PLOT_STUFF = 1 linestyles = ['-*', '--*', ':*', '-.*'] system = conf.system r = loadUR_urdf() robot = RobotWrapper(r.model, r.collision_model, r.visual_model) # Initialize parameters nq, nv = robot.nq, robot.nv # joint position and velocity size n = nq + nv # state size m = robot.na # control sizecost_scale U = np.zeros((N, m)) # initial guess for control inputs # create simulator simu = RobotSimulator(conf, robot) # Initialize the trajectory that we would like to track traj = TrajectoryCirc(conf.T) # traj = TrajectoryLin(conf.p0, conf.p_des, conf.T) traj.compute(0) p_0 = np.zeros(3) np.copyto(p_0, traj.pos) # desired position # evaluate inverse kinematics for q0 IK = InverseKinematic(robot, eps=1e-3, MAX_ITER=10000) M_0 = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) # desired orientation q_0 = IK.compute(p_0, M_0, conf.q0) x0 = np.concatenate((q_0, np.zeros(6))) # initial states