# 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