def test_jacobian_api_negative(self):
def g_good(x):
return [x[0], x[1]]
x_bad = [[1, 2]]
with self.assertRaises(AssertionError) as cm:
jacobian(g_good, x_bad)
self.assertIn("x must be a vector", str(cm.exception))
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
'''
YOUR CODE HERE
'''
u = np.zeros(self.nu)
return u
def test_eval_binding(self):
qp = TestQP()
prog = qp.prog
x = qp.x
x_expected = np.array([1., 1.])
costs = qp.costs
cost_values_expected = [2., 1.]
constraints = qp.constraints
constraint_values_expected = [1., 1., 2., 3.]
with catch_drake_warnings(action='ignore'):
prog.Solve()
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
enum = zip(constraints, constraint_values_expected)
for (constraint, value_expected) in enum:
value = prog.EvalBindingAtSolution(constraint)
self.assertTrue(np.allclose(value, value_expected))
enum = zip(costs, cost_values_expected)
for (cost, value_expected) in enum:
value = prog.EvalBindingAtSolution(cost)
self.assertTrue(np.allclose(value, value_expected))
# Existence check for non-autodiff versions.
self.assertIsInstance(
prog.EvalBinding(costs[0], x_expected), np.ndarray)
self.assertIsInstance(
prog.EvalBindings(prog.GetAllConstraints(), x_expected),
np.ndarray)
# Existence check for autodiff versions.
self.assertIsInstance(
jacobian(partial(prog.EvalBinding, costs[0]), x_expected),
np.ndarray)
self.assertIsInstance(
jacobian(partial(prog.EvalBindings, prog.GetAllConstraints()),
x_expected),
np.ndarray)
# Bindings for `Eval`.
x_list = (float(1.), AutoDiffXd(1.), sym.Variable("x"))
T_y_list = (float, AutoDiffXd, sym.Expression)
evaluator = costs[0].evaluator()
for x_i, T_y_i in zip(x_list, T_y_list):
y_i = evaluator.Eval(x=[x_i, x_i])
self.assertIsInstance(y_i[0], T_y_i)
def test_kinematics_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
def test_gradient_and_jacobian(self):
# Explicitly test with type(x) == list, so that we ensure the API
# handles this case.
x = [1., 2.]
def f(x):
return x.dot(x)
def f_vector(x):
# Return a vector of size 1.
return [f(x)]
def df(x):
# N.B. We explicitly convert to ndarray since we call this
# directly.
x = np.asarray(x)
return 2 * x
def g_vector(x):
# This will be a vector of size 2.
return [f(x), 3 * f(x)]
def dg_vector(x):
# This will be a 2x2 matrix.
return [df(x), 3 * df(x)]
def g_matrix(x):
return [g_vector(x)]
def dg_matrix(x):
return [dg_vector(x)]
numpy_compare.assert_equal(gradient(f, x), df(x))
numpy_compare.assert_equal(gradient(f_vector, x), df(x))
numpy_compare.assert_equal(jacobian(f, x), df(x))
numpy_compare.assert_equal(jacobian(g_vector, x), dg_vector(x))
numpy_compare.assert_equal(jacobian(g_matrix, x), dg_matrix(x))
def test_gradient(self):
r = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/Pendulum/Pendulum.urdf"),
pydrake.rbtree.FloatingBaseType.kRollPitchYaw)
def do_transform(q):
kinsol = r.doKinematics(q)
point = np.ones((3, 1))
return r.transformPoints(kinsol, point, 2, 0)
q = np.zeros(7)
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones((3, 1))))
g = fd.jacobian(do_transform, q)
self.assertTrue(np.allclose(g,
np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])))
def test_gradient(self):
r = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"),
pydrake.rbtree.FloatingBaseType.kRollPitchYaw)
def do_transform(q):
kinsol = r.doKinematics(q)
point = np.ones((3, 1))
return r.transformPoints(kinsol, point, 2, 0)
q = np.zeros(7)
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones((3, 1))))
g = fd.jacobian(do_transform, q)
self.assertTrue(np.allclose(g,
np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])))
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
mp = MathematicalProgram()
#control variables = u = self.nu x 1
#ddot{q} as variable w/ q's shape
controls = mp.NewContinuousVariables(self.nu, "controls")
qdd = mp.NewContinuousVariables(q.shape[0], "qdd")
#make variables for lambda's and beta's
k = 0
b = mp.NewContinuousVariables(2, "betas_%d" % k)
betas = b
for i in range(len(self.grasp_points)):
b = mp.NewContinuousVariables(
2, "betas_%d" % k) #pair of betas for each contact point
betas = np.vstack((betas, b))
mp.AddLinearConstraint(
betas[i, 0] >= 0).evaluator().set_description(
"beta_0 greater than 0 for %d" % i)
mp.AddLinearConstraint(
betas[i, 1] >= 0).evaluator().set_description(
"beta_1 greater than 0 for %d" % i)
#c_0=normals+mu*tangent
#c1 = normals-mu*tangent
n = grasp_normals_world_now.T[:, 0:2] #row=contact point?
t = get_perpendicular2d(n[0])
c_i1 = n[0] + np.dot(self.mu, t)
c_i2 = n[0] - np.dot(self.mu, t)
l = c_i1.dot(betas[0, 0]) + c_i2.dot(betas[0, 1])
lambdas = l
for i in range(1, len(self.grasp_points)):
n = grasp_normals_world_now.T[:, 0:
2] #row=contact point? transpose then index
t = get_perpendicular2d(n[i])
c_i1 = n[i] - np.dot(self.mu, t)
c_i2 = n[i] + np.dot(self.mu, t)
l = c_i1.dot(betas[i, 0]) + c_i2.dot(betas[i, 1])
lambdas = np.vstack((lambdas, l))
control_period = 0.0333
#Constrain the dyanmics
dyn = M.dot(qdd) + C - B.dot(controls)
for i in range(q.shape[0]):
j_c = 0
for l in range(len(self.grasp_points)):
j_sub = J_contact[:, l, :].T
j_c += j_sub.dot(lambdas[l])
# print(j_c.shape)
# print(dyn.shape)
mp.AddConstraint(dyn[i] - j_c[i] == 0)
#PD controller using kinematics
Kp = 100.0
Kd = 1.0
control_period = 0.0333
next_q = q + v.dot(control_period) + np.dot(qdd.dot(control_period**2),
.5)
next_q_dot = v + qdd.dot(control_period)
mp.AddQuadraticCost(Kp * (qdes - next_q).dot((qdes - next_q).T) + Kd *
(next_q_dot).dot(next_q_dot.T))
Kp_ext = 100.
mp.AddQuadraticCost(Kp_ext * (qdes - next_q)[-3:].dot(
(qdes - next_q)[-3:].T))
res = Solve(mp)
c = res.GetSolution(controls)
return c
def CallLQR(x, u, Q, R):
f_x_u = jacobian(self.CalcF, np.hstack((x, u)))
A = f_x_u[:, 0:self.n]
B = f_x_u[:, self.n:self.n + self.m]
K, P = LinearQuadraticRegulator(A, B, Q, R)
return K, P
def one_rotor_loss():
# simulate quadrotor w/ LQR controller using forward Euler integration.
# fixed point
n = 12
m = 4
xd = np.zeros(n)
xd[0:2] = [2, 1]
ud = np.zeros(m)
ud[:] = mass * g / 4
x_u = np.hstack((xd, ud))
partials = jacobian(CalcF, x_u)
A0 = partials[:, 0:n]
print(A0)
B0 = partials[:, n:n + m]
print(B0)
Q = 10 * np.eye(n)
R = np.eye(m)
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(A0, B0, Q, R)
# simulate stabilizing about fixed point using LQR controller
# dt = 0.001
# N = int(2.0/dt)
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x[0] = x0
timeVec = np.zeros(2 * N + 1)
all_p = np.zeros(2 * N)
all_q = np.zeros(2 * N)
# keeping track of all forces on the quadrotors to show how they adapt
forces = np.zeros((2 * N, 4))
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i] = x_u[9]
all_q[i] = x_u[10]
forces[i] = x_u[12:16]
print(forces[i])
x[i + 1] = x[i] + dt * CalcF(x_u)
timeVec[i] = timeVec[i - 1] + dt
#%% open meshact
# vis = meshcat.Visualizer()
# vis.open
#%% Meshcat animation
# PlotTrajectoryMeshcat(x, timeVec, vis)
# at timestamp N=5.0 seconds, rotor 4 fails and we switch control systems
n = 6
m = 2
#for failure of rotor 4, omega_hat_4 = 0
n_bar = [0.0, 0.289, 0.958]
sigma_motor = 0.015
#from eq 51
xd = np.zeros(n)
xd[1] = 5.69 #q
xd[2] = n_bar[0] #nx
xd[3] = n_bar[1] #ny
ud = np.zeros(m) #zero because u is in error coordinates
x_u = np.hstack((xd, ud))
#based on eq 51
force_bar = np.array([2.05, 1.02, 2.05, 0])
# force_bar = np.array([1.02, 2.05, 1.02, 0])
omega_bar = np.sqrt(force_bar / kF)
#define the A matrix for failed quad
Ae = np.zeros([6, 6])
B0_f = np.zeros([4, 2])
#extended state
Be = np.zeros([6, 2])
Q = np.eye(n)
R = np.eye(m)
r_hat = ((kF * kM) / gamma) * (omega_bar[0]**2 - omega_bar[1]**2 +
omega_bar[2]**2 - omega_bar[3]**2)
#define coupling constant
a_bar = (I[0, 0] - I[2, 2]) / I[0, 0]
B0_f[1, 0] = 1
B0_f[0, 1] = 1
B0_f = (l / I[0, 0]) * B0_f
Ae[0, 1] = a_bar
Ae[1, 0] = -1 * a_bar
Ae[1, 0] = -1 * a_bar
Ae[2, 1] = -1 * n_bar[2]
Ae[2, 3] = r_hat
Ae[3, 0] = n_bar[2]
Ae[3, 2] = -1 * r_hat
Ae[0:4, 4:6] = B0_f
Ae[4:6, 4:6] = -1 * np.eye(2) / sigma_motor
Be[4:6, 0:2] = np.eye(2) / sigma_motor
#the failed matrix u
# u_f = np.zeros([2,1])
Q[2:4, 2:4] *= 20
Q[4:6, 4:6] *= 0
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(Ae, Be, Q, R)
# simulate stabilizing about fixed point using LQR controller
x_mesh = np.zeros((N + 1, 12))
x_mesh[0] = x[N]
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x0[0] = 5
x0[1] = 5
x[0] = x0
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i + N] = x[i, 0]
all_q[i + N] = x[i, 1]
xDot = Ae.dot(x[i]) + Be.dot(x_u[6:8])
x[i + 1] = x[i] + dt * xDot
# print(x[i,1])
u_i = -K0.dot(x[i] - xd) + ud
f = get_forces(u_i, force_bar)
# x_mesh[i+1] = x_mesh[i] + dt*CalcF(np.hstack((x_mesh[i], f)))
forces[i + N] = f
timeVec[i + N] = timeVec[i - 1 + N] + dt
x_mesh[i + 1] = x_mesh[i] + dt * CalcF(np.hstack([x_mesh[i], f]))
x_mesh[i + 1, 9] = x[i + 1, 0]
x_mesh[i + 1, 10] = x[i + 1, 1]
print(x_mesh[i + 1])
# plot_forces(forces, timeVec, all_p, all_q, 1, force_bar, omega_bar[1])
#%% open meshact
vis = meshcat.Visualizer()
vis.open
#%% Meshcat animation
PlotTrajectoryMeshcat(x_mesh, timeVec[N:2 * N + 1], vis)
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(
self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(
self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
number_of_grasp_points = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
# print('shape' + str(np.shape(qdes)))
# print('self.nq' + str(self.nq))
# print('self.nu' + str(self.nu))
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
kd = 1
kp = 25
from pydrake.all import MathematicalProgram, Solve
mp = MathematicalProgram()
u = mp.NewContinuousVariables(self.nu, "u")
qdd = mp.NewContinuousVariables(self.nq, "qdd")
x_y_dimensions = 2
lambda_variable = mp.NewContinuousVariables(x_y_dimensions, number_of_grasp_points, "lambda_variable")
forces = np.zeros((self.nq))
for i in range(number_of_grasp_points):
current_forces = np.transpose(J_contact)[:,i,:].dot(lambda_variable[:,i])
forces = forces + current_forces
leftHandSide = M.dot(qdd) + C
rightHandSide = B.dot(u) + forces
for i in range(len(leftHandSide)):
mp.AddConstraint(leftHandSide[i] == rightHandSide[i])
# Calculate Normals
normals = np.array([])
for i in range(number_of_grasp_points):
current_grasp_normal = grasp_normals_world_now[0:2, i]
normals = np.vstack((normals, current_grasp_normal)) if normals.size else current_grasp_normal
# Calculate Tangeants
tangeants = np.array([])
for i in range(number_of_grasp_points):
current_grasp_tangeant = np.array([normals[i, 1], -normals[i, 0]])
tangeants = np.vstack((tangeants, current_grasp_tangeant)) if tangeants.size else current_grasp_tangeant
# Create beta variables
beta = mp.NewContinuousVariables(2, number_of_grasp_points, "b0")
# Create Grasps
for i in range(number_of_grasp_points):
c0 = normals[i] - self.mu * tangeants[i]
c1 = normals[i] + self.mu * tangeants[i]
right_hand_lambda1 = c0 * beta[0, i] + c1 * beta[1, i]
mp.AddConstraint(lambda_variable[0, i] == right_hand_lambda1[0])
mp.AddConstraint(lambda_variable[1, i] == right_hand_lambda1[1])
mp.AddConstraint(beta[0, i] >= 0)
mp.AddConstraint(beta[1, i] >= 0)
mp.AddConstraint(normals[i].dot(lambda_variable[:, i]) >= 0.25)
# Copying the control period of the constructor. Probably not supposed to do this...
next_tick_qd = v + qdd * self.cont¨rol_period
next_tick_q = q + next_tick_qd * self.control_period
q_error = qdes - next_tick_q
proportionalCost = q_error.dot(np.transpose(q_error))
qd_error = 0 - next_tick_qd
diffCost = qd_error.dot(np.transpose(qd_error))
mp.AddQuadraticCost(kp * proportionalCost + kd * diffCost)
result = Solve(mp)
u_solution = result.GetSolution(u)
u = np.zeros(self.nu)
return u_solution
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x)
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(
self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(
self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
normals = grasp_normals_world_now[0:2, :].T
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
'''
YOUR CODE HERE
'''
# This is not proper orthogonal vector...but it somehow makes it work...
# whereas the correct does not
def ortho2(x):
return np.array([x[0],-x[1]])
#return np.array([x[1],-x[0]])
prog = MathematicalProgram()
dt = self.control_period
u = prog.NewContinuousVariables(self.nu, "u")
qdd = prog.NewContinuousVariables(q.shape[0], "qdd")
for i in range(qdd.shape[0]):
prog.AddLinearConstraint(qdd[i] <= 40)
prog.AddLinearConstraint(qdd[i] >= -40)
#prog.AddLinearConstraint(v[i] + qdd[i] * dt <= 80)
#prog.AddLinearConstraint(v[i] - qdd[i] * dt >= -80)
beta = prog.NewContinuousVariables(n_cf, 2, "beta")
#prog.AddQuadraticCost(0.1*np.sum(beta**2))
for i in range(n_cf):
prog.AddLinearConstraint(beta[i,0] >= 0)
prog.AddLinearConstraint(beta[i,1] >= 0)
prog.AddLinearConstraint(beta[i,0] <= 10)
prog.AddLinearConstraint(beta[i,1] <= 10)
c = np.zeros((n_cf,2,2))
for i in range(n_cf):
c[i,0] = normals[i] - self.mu * ortho2(normals[i])
c[i,1] = normals[i] + self.mu * ortho2(normals[i])
const = M.dot(qdd) + C - B.dot(u) ## eq 0
for i in range(n_cf):
lambda_i = c[i,0]*beta[i,0] + c[i,1]*beta[i,1]
tmp = J_contact[:, i, :].T.dot(lambda_i)
const -= tmp
for i in range(const.shape[0]):
prog.AddLinearConstraint(const[i] == 0.0)
Kp = 1000
Kd = 10
#v2 = v + dt * qdd
#q2 = q + v * dt + 0.5 * qdd * dt*dt
v2 = v + dt * qdd
q2 = q + (v+v2)/2 * dt #+ 0.5 * qdd * dt*dt
#v2 = v + dt * qdd
#q2 = q + v2 * dt
prog.AddQuadraticCost(Kp * np.sum((qdes - q2) ** 2) + Kd * np.sum(v2**2))
result = prog.Solve()
u = prog.GetSolution(u)
return u
def CalcTrajectory(self, traj_specs, t0=0., is_logging_trajectories=True):
assert (traj_specs.xd.shape == (self.n, ))
assert (traj_specs.ud.shape == (self.m, ))
def CallLQR(x, u, Q, R):
f_x_u = jacobian(self.CalcF, np.hstack((x, u)))
A = f_x_u[:, 0:self.n]
B = f_x_u[:, self.n:self.n + self.m]
K, P = LinearQuadraticRegulator(A, B, Q, R)
return K, P
# linearize about target/goal position
if traj_specs.QN is None:
Kd, traj_specs.QN = CallLQR(traj_specs.xd, traj_specs.ud, \
traj_specs.Q, traj_specs.R)
self.traj_specs = traj_specs
# calculates lx
def CalcLx(xi, i, t0):
Lx = traj_specs.Q.dot(xi - traj_specs.xd)
# add contribution from waypoint weights
if not (traj_specs.xw_list is None):
for xw in traj_specs.xw_list:
Lx += xw.W.dot(xi - xw.x) * self.discount(xw, i, t0)
return Lx
# calculates lxx
def CalcLxx(i, t0):
Lxx = traj_specs.Q.copy()
if not (traj_specs.xw_list is None):
for xw in traj_specs.xw_list:
Lxx += xw.W * self.discount(xw, i, t0)
return Lxx
# allocate storage for derivatives
Qx = np.zeros((traj_specs.N, self.n))
Qxx = np.zeros((traj_specs.N, self.n, self.n))
Qu = np.zeros((traj_specs.N, self.m))
Quu = np.zeros((traj_specs.N, self.m, self.m))
Qux = np.zeros((traj_specs.N, self.m, self.n))
delta_V = np.zeros(traj_specs.N + 1)
Vx = np.zeros((traj_specs.N + 1, self.n))
Vxx = np.zeros((traj_specs.N + 1, self.n, self.n))
k = np.zeros((traj_specs.N, self.m))
K = np.zeros((traj_specs.N, self.m, self.n))
# storage for trajectories
x = np.zeros((traj_specs.N + 1, self.n))
u = np.zeros((traj_specs.N, self.m))
x_next = x.copy()
u_next = u.copy()
x[0] = traj_specs.x0
'''
initialize first trajectory by
simulating forward with LQR controller about x0.
'''
x0 = np.zeros(self.n)
x0[0:3] = traj_specs.x0[0:3]
K0, P0 = CallLQR(x0, traj_specs.u0, traj_specs.Q, traj_specs.R)
for i in range(traj_specs.N):
u[i] = -K0.dot(x[i] - traj_specs.x0) + traj_specs.u0
x_u = np.hstack((x[i], u[i]))
x[i + 1] = x[i] + traj_specs.h * self.CalcF(x_u)
'''
initialize first trajectory by
simulating forward with LQR controller about xd.
'''
# Kd, Qd = CallLQR(traj_specs.xd, traj_specs.ud, traj_specs.Q, traj_specs.R)
# for i in range(traj_specs.N):
# u[i] = -Kd.dot(x[i]-traj_specs.xd) + traj_specs.ud
# x_u = np.hstack((x[i], u[i]))
# x[i+1] = x[i] + traj_specs.h*self.CalcF(x_u)
# logging
max_iterations = 5 # number of maximum iterations (forward + backward passes)
J = np.zeros(max_iterations + 1)
J[0] = self.CalcJ(x, u, t0)
print "initial cost: ", J[0]
if is_logging_trajectories:
x_log = x.copy()
u_log = u.copy()
x_log.resize(1, x.shape[0], x.shape[1])
u_log.resize(1, u.shape[0], u.shape[1])
# It really should be a while loop, but for linear systems one
# iteration seems sufficient. And I am sure this can be proven.
# And for quadrotors it usually takes less than 5 iterations
# to converge.
j = 0 # iteration index
while True:
# initialize boundary conditions
Vxx[traj_specs.N] = traj_specs.QN
Vx[traj_specs.N] = traj_specs.QN.dot(x[traj_specs.N] -
traj_specs.xd)
# backward pass
for i in range(traj_specs.N - 1, -1, -1): # i = N-1, ....
lx = CalcLx(x[i], i, t0)
lu = traj_specs.R.dot(u[i] - traj_specs.ud)
lxx = CalcLxx(i, t0)
luu = traj_specs.R
x_u = np.hstack((x[i], u[i]))
f_x_u = jacobian(self.CalcF, x_u)
fx = traj_specs.h * f_x_u[:, 0:self.n] + np.eye(self.n)
fu = traj_specs.h * f_x_u[:, self.n:self.n + self.m]
Qx[i] = lx + fx.T.dot(Vx[i + 1])
Qu[i] = lu + fu.T.dot(Vx[i + 1])
Qxx[i] = lxx + fx.T.dot(Vxx[i + 1].dot(fx))
Quu[i] = luu + fu.T.dot(Vxx[i + 1].dot(fu))
Qux[i] = fu.T.dot(Vxx[i + 1].dot(fx))
# compute k and K
k[i] = -LA.solve(Quu[i], Qu[i]) # Quu_inv.dot(Qu[i])
K[i] = -LA.solve(Quu[i], Qux[i]) # Quu_inv.dot(Qux[i])
# update derivatives of V
# delta_V[i] = 0.5*Qu[i].dot(k[i])
# Vx[i] = Qx[i] + Qu[i].dot(K[i])
# Vxx[i] = Qxx[i] + Qux[i].T.dot(K[i])
delta_V[i] = 0.5 * k[i].dot(Quu[i].dot(k[i])) + Qu[i].dot(k[i])
Vx[i] = Qx[i] + K[i].T.dot(Quu[i].dot(k[i])) + K[i].T.dot(
Qu[i]) + Qux[i].T.dot(k[i])
Vxx[i] = Qxx[i] + K[i].T.dot(Quu[i].dot(K[i])) + K[i].T.dot(
Qux[i]) + Qux[i].T.dot(K[i])
# forward pass
del i
x_next[0] = x[0]
alpha = 1
line_search_count = 0
while True:
for t in range(traj_specs.N):
u_next[t] = u[t] + alpha * k[t] + K[t].dot(x_next[t] -
x[t])
x_u = np.hstack((x_next[t], u_next[t]))
x_next[t + 1] = x_next[t] + traj_specs.h * self.CalcF(x_u)
J_new = self.CalcJ(x_next, u_next, t0=t0, i0=0)
if J_new <= J[j]:
J[j + 1] = J_new
x = x_next.copy()
u = u_next.copy()
break
elif line_search_count > 5:
J[j + 1] = J_new
break
else:
alpha *= 0.5
line_search_count += 1
if is_logging_trajectories:
x_log = np.append(x_log,
x.reshape(1, x.shape[0], x.shape[1]),
axis=0)
u_log = np.append(u_log,
u.reshape(1, u.shape[0], u.shape[1]),
axis=0)
print "Iteration ", j, ", line search steps: ", line_search_count, ", J: ", J[
j + 1]
cost_reduction = (J[j] - J[j + 1]) / J[j]
j += 1
if j >= max_iterations or cost_reduction < 0.01 or line_search_count > 5:
break
if is_logging_trajectories:
return x_log, u_log, J[0:j + 1], traj_specs.QN, Vx, Vxx, k, K
else:
return x, u, J[0:j + 1], traj_specs.QN, Vx, Vxx, k, K
def test_kinematics_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# Ensure mismatched sizes throw an error.
q_bad = np.zeros(num_q + 1)
v_bad = np.zeros(num_v + 1)
bad_args_list = (
(q_bad,),
(q_bad, v),
(q, v_bad),
)
for bad_args in bad_args_list:
with self.assertRaises(SystemExit):
tree.doKinematics(*bad_args)
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([
[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([
[0, 0, 1, 0],
[0, 1, 0, 0],
[-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Relative RPY, checking autodiff (#9886).
q[:] = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
q_ad = np.array([AutoDiffXd(x) for x in q])
world = tree.findFrame("world")
frame = tree.findFrame("arm_com")
kinsol = tree.doKinematics(q)
rpy = tree.relativeRollPitchYaw(
cache=kinsol, from_body_or_frame_ind=world.get_frame_index(),
to_body_or_frame_ind=frame.get_frame_index())
kinsol_ad = tree.doKinematics(q_ad)
rpy_ad = tree.relativeRollPitchYaw(
cache=kinsol_ad, from_body_or_frame_ind=world.get_frame_index(),
to_body_or_frame_ind=frame.get_frame_index())
for x, x_ad in zip(rpy, rpy_ad):
self.assertEqual(x, x_ad.value())
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([
[0, 0, 1, 0],
[0, 1, 0, 0],
[-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Geometric Jacobian.
# Construct a new tree with a quaternion floating base.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"),
floating_base_type=FloatingBaseType.kQuaternion)
num_q = 8
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = tree.getZeroConfiguration()
v = np.zeros(num_v)
kinsol = tree.doKinematics(q, v)
# - Sanity check sizes.
J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0)
J_eeDotTimesV = tree.geometricJacobianDotTimesV(kinsol, 0, 2, 0)
self.assertEqual(J_default.shape[0], 6)
self.assertEqual(J_default.shape[1], num_v)
self.assertEqual(len(v_indices_default), num_v)
self.assertEqual(J_eeDotTimesV.shape[0], 6)
# - Check QDotToVelocity and VelocityToQDot methods
q = tree.getZeroConfiguration()
v_real = np.zeros(num_v)
q_ad = np.array(list(map(AutoDiffXd, q)))
v_real_ad = np.array(list(map(AutoDiffXd, v_real)))
kinsol = tree.doKinematics(q)
kinsol_ad = tree.doKinematics(q_ad)
qd = tree.transformVelocityToQDot(kinsol, v_real)
v = tree.transformQDotToVelocity(kinsol, qd)
qd_ad = tree.transformVelocityToQDot(kinsol_ad, v_real_ad)
v_ad = tree.transformQDotToVelocity(kinsol_ad, qd_ad)
self.assertEqual(qd.shape, (num_q,))
self.assertEqual(v.shape, (num_v,))
self.assertEqual(qd_ad.shape, (num_q,))
self.assertEqual(v_ad.shape, (num_v,))
v_to_qdot = tree.GetVelocityToQDotMapping(kinsol)
qdot_to_v = tree.GetQDotToVelocityMapping(kinsol)
v_to_qdot_ad = tree.GetVelocityToQDotMapping(kinsol_ad)
qdot_to_v_ad = tree.GetQDotToVelocityMapping(kinsol_ad)
self.assertEqual(v_to_qdot.shape, (num_q, num_v))
self.assertEqual(qdot_to_v.shape, (num_v, num_q))
self.assertEqual(v_to_qdot_ad.shape, (num_q, num_v))
self.assertEqual(qdot_to_v_ad.shape, (num_v, num_q))
v_map = tree.transformVelocityMappingToQDotMapping(kinsol,
np.eye(num_v))
qd_map = tree.transformQDotMappingToVelocityMapping(kinsol,
np.eye(num_q))
v_map_ad = tree.transformVelocityMappingToQDotMapping(kinsol_ad,
np.eye(num_v))
qd_map_ad = tree.transformQDotMappingToVelocityMapping(kinsol_ad,
np.eye(num_q))
self.assertEqual(v_map.shape, (num_v, num_q))
self.assertEqual(qd_map.shape, (num_q, num_v))
self.assertEqual(v_map_ad.shape, (num_v, num_q))
self.assertEqual(qd_map_ad.shape, (num_q, num_v))
# - Check FindBody and FindBodyIndex methods
body_name = "arm"
body = tree.FindBody(body_name)
self.assertEqual(body.get_body_index(), tree.FindBodyIndex(body_name))
# - Check ChildOfJoint methods
body = tree.FindChildBodyOfJoint("theta")
self.assertIsInstance(body, RigidBody)
self.assertEqual(body.get_name(), "arm")
self.assertEqual(tree.FindIndexOfChildBodyOfJoint("theta"), 2)
# - Check that default value for in_terms_of_qdot is false.
J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, False)
self.assertTrue((J_default == J_not_in_terms_of_q_dot).all())
self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot)
# - Check with in_terms_of_qdot set to True.
J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, True)
self.assertEqual(J_in_terms_of_q_dot.shape[0], 6)
self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q)
self.assertEqual(len(v_indices_in_terms_of_qdot), num_q)
# - Check transformPointsJacobian methods
q = tree.getZeroConfiguration()
v = np.zeros(num_v)
kinsol = tree.doKinematics(q, v)
Jv = tree.transformPointsJacobian(cache=kinsol, points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1,
in_terms_of_qdot=False)
Jq = tree.transformPointsJacobian(cache=kinsol, points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1,
in_terms_of_qdot=True)
JdotV = tree.transformPointsJacobianDotTimesV(cache=kinsol,
points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1)
self.assertEqual(Jv.shape, (3, num_v))
self.assertEqual(Jq.shape, (3, num_q))
self.assertEqual(JdotV.shape, (3,))
# - Check that drawKinematicTree runs
tree.drawKinematicTree(
join(os.environ["TEST_TMPDIR"], "test_graph.dot"))
# - Check relative twist method
twist = tree.relativeTwist(cache=kinsol,
base_or_frame_ind=0,
body_or_frame_ind=1,
expressed_in_body_or_frame_ind=0)
self.assertEqual(twist.shape[0], 6)
def test_kinematics_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([
[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([
[0, 0, 1, 0],
[0, 1, 0, 0],
[-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([
[0, 0, 1, 0],
[0, 1, 0, 0],
[-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Geometric Jacobian.
# Construct a new tree with a quaternion floating base.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"),
floating_base_type=FloatingBaseType.kQuaternion)
num_q = 8
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = tree.getZeroConfiguration()
kinsol = tree.doKinematics(q)
# - Sanity check sizes.
J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0)
self.assertEqual(J_default.shape[0], 6)
self.assertEqual(J_default.shape[1], num_v)
self.assertEqual(len(v_indices_default), num_v)
# - Check that default value for in_terms_of_qdot is false.
J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, False)
self.assertTrue((J_default == J_not_in_terms_of_q_dot).all())
self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot)
# - Check with in_terms_of_qdot set to True.
J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, True)
self.assertEqual(J_in_terms_of_q_dot.shape[0], 6)
self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q)
self.assertEqual(len(v_indices_in_terms_of_qdot), num_q)
def two_rotor_loss():
# simulate quadrotor w/ LQR controller using forward Euler integration.
# fixed point
n = 12
m = 4
xd = np.zeros(n)
xd[0:2] = [2, 1]
ud = np.zeros(m)
ud[:] = mass * g / 4
x_u = np.hstack((xd, ud))
partials = jacobian(CalcF, x_u)
A0 = partials[:, 0:n]
B0 = partials[:, n:n + m]
Q = 10 * np.eye(n)
R = np.eye(m)
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(A0, B0, Q, R)
# simulate stabilizing about fixed point using LQR controller
# dt = 0.001
# N = int(2.0/dt)
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x[0] = x0
timeVec = np.zeros(2 * N + 1)
all_p = np.zeros(2 * N)
all_q = np.zeros(2 * N)
# keeping track of all forces on the quadrotors to show how they adapt
forces = np.zeros((2 * N, 4))
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i] = x_u[9]
all_q[i] = x_u[10]
forces[i] = x_u[12:16]
print(forces[i])
x[i + 1] = x[i] + dt * CalcF(x_u)
timeVec[i] = timeVec[i - 1] + dt
#%% open meshact
# vis = meshcat.Visualizer()
# vis.open
#%% Meshcat animation
# PlotTrajectoryMeshcat(x, timeVec, vis)
#for failure of rotor 4 and 2, omega_hat_4 = omega_hat_2= 0
n = 5
m = 1
kF = 6.41e-6
n_bar = [0.0, 0.0, 1.0]
sigma_motor = 0.015
#from eq 51
xd = np.zeros(n) #p,q are set to 0
xd[2] = n_bar[0] #nx
xd[3] = n_bar[1] #ny
ud = np.zeros(m) #zero because u is in error coordinates
x_u = np.hstack((xd, ud))
#based on eq 51
force_bar = np.array([2.45, 0.0, 2.45, 0.0])
#omega_bar = np.sqrt(force_bar/kF) explicitly calcualted in paper
omega_bar = [-619.0, 0.0, -619.0, 0.0]
#define the A matrix for failed quad
Ae = np.zeros([5, 5])
B0_f = np.zeros([4, 1])
#extended state
Be = np.zeros([5, 1])
Q = np.eye(n)
R = np.eye(m)
R *= 0.75
r_hat = ((kF * kM) / gamma) * (omega_bar[0]**2 - omega_bar[1]**2 +
omega_bar[2]**2 - omega_bar[3]**2)
#define coupling constant
a_bar = (I[0, 0] - I[2, 2]) / I[0, 0]
B0_f[1] = 1
B0_f = (l / I[0, 0]) * B0_f
Ae[0, 1] = a_bar
Ae[1, 0] = -1 * a_bar
Ae[1, 0] = -1 * a_bar
Ae[2, 1] = -1 * n_bar[2]
Ae[2, 3] = r_hat
Ae[3, 0] = n_bar[2]
Ae[3, 2] = -1 * r_hat
Ae[0:4, 4:5] = B0_f
Ae[4, 4] = -1 * sigma_motor
Be[4] = 1 / sigma_motor
#the failed matrix u
# u_f = np.zeros([2,1])
Q[2, 2] *= 1000
Q[3, 3] *= 2
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(Ae, Be, Q, R)
# simulate stabilizing about fixed point using LQR controller
# dt = 0.001
# N = int(5.0/dt)
x = np.zeros((N + 1, n))
# forces = np.zeros((N+1, 4))
# forces[0] = np.zeros(4)
x0 = np.zeros(n)
x0[0] = 5
x0[1] = 5
x[0] = x0
# here, assume the nx and ny are initially zero
# additional motor values are also set to zero
# timeVec = np.zeros(N+1)
for i in range(N):
x_u = np.hstack((x[i], -K0.dot(x[i] - xd) + ud))
all_p[i + N] = x[i, 0]
all_q[i + N] = x[i, 1]
xDot = Ae.dot(x[i]) + Be.dot(x_u[n:n + m])
x[i + 1] = x[i] + dt * xDot
# print(x[i,1])
u_i = -K0.dot(x[i] - xd) + ud
f = np.zeros(4)
f[2] = (u_i[0] + 2 * force_bar[2]) / 2
f[0] = np.sum(force_bar) - f[2]
forces[i + N] = f
timeVec[i + N] = timeVec[i - 1 + N] + dt
# PlotFailedTraj(x.copy(), dt, xd, ud, timeVec, forces)
print(timeVec.shape)
print(force_bar.shape)
plot_forces(forces, timeVec, all_p, all_q, 2, force_bar, omega_bar[1])
def test_kinematics_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([
[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([
[0, 0, 1, 0],
[0, 1, 0, 0],
[-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([
[0, 0, 1, 0],
[0, 1, 0, 0],
[-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Geometric Jacobian.
# Construct a new tree with a quaternion floating base.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"),
floating_base_type=FloatingBaseType.kQuaternion)
num_q = 8
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = tree.getZeroConfiguration()
kinsol = tree.doKinematics(q)
# - Sanity check sizes.
J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0)
self.assertEqual(J_default.shape[0], 6)
self.assertEqual(J_default.shape[1], num_v)
self.assertEqual(len(v_indices_default), num_v)
# - Check QDotToVelocity and VelocityToQDot methods
q = tree.getZeroConfiguration()
v_real = np.zeros(num_v)
q_ad = np.array(map(AutoDiffXd, q))
v_real_ad = np.array(map(AutoDiffXd, v_real))
kinsol = tree.doKinematics(q)
kinsol_ad = tree.doKinematics(q_ad)
qd = tree.transformVelocityToQDot(kinsol, v_real)
v = tree.transformQDotToVelocity(kinsol, qd)
qd_ad = tree.transformVelocityToQDot(kinsol_ad, v_real_ad)
v_ad = tree.transformQDotToVelocity(kinsol_ad, qd_ad)
self.assertEqual(qd.shape, (num_q,))
self.assertEqual(v.shape, (num_v,))
self.assertEqual(qd_ad.shape, (num_q,))
self.assertEqual(v_ad.shape, (num_v,))
v_to_qdot = tree.GetVelocityToQDotMapping(kinsol)
qdot_to_v = tree.GetQDotToVelocityMapping(kinsol)
v_to_qdot_ad = tree.GetVelocityToQDotMapping(kinsol_ad)
qdot_to_v_ad = tree.GetQDotToVelocityMapping(kinsol_ad)
self.assertEqual(v_to_qdot.shape, (num_q, num_v))
self.assertEqual(qdot_to_v.shape, (num_v, num_q))
self.assertEqual(v_to_qdot_ad.shape, (num_q, num_v))
self.assertEqual(qdot_to_v_ad.shape, (num_v, num_q))
v_map = tree.transformVelocityMappingToQDotMapping(kinsol,
np.eye(num_v))
qd_map = tree.transformQDotMappingToVelocityMapping(kinsol,
np.eye(num_q))
v_map_ad = tree.transformVelocityMappingToQDotMapping(kinsol_ad,
np.eye(num_v))
qd_map_ad = tree.transformQDotMappingToVelocityMapping(kinsol_ad,
np.eye(num_q))
self.assertEqual(v_map.shape, (num_v, num_q))
self.assertEqual(qd_map.shape, (num_q, num_v))
self.assertEqual(v_map_ad.shape, (num_v, num_q))
self.assertEqual(qd_map_ad.shape, (num_q, num_v))
# - Check ChildOfJoint methods
body = tree.FindChildBodyOfJoint("theta")
self.assertIsInstance(body, RigidBody)
self.assertEqual(body.get_name(), "arm")
self.assertEqual(tree.FindIndexOfChildBodyOfJoint("theta"), 2)
# - Check that default value for in_terms_of_qdot is false.
J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, False)
self.assertTrue((J_default == J_not_in_terms_of_q_dot).all())
self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot)
# - Check with in_terms_of_qdot set to True.
J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, True)
self.assertEqual(J_in_terms_of_q_dot.shape[0], 6)
self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q)
self.assertEqual(len(v_indices_in_terms_of_qdot), num_q)
# - Check that drawKinematicTree runs
tree.drawKinematicTree(
join(os.environ["TEST_TMPDIR"], "test_graph.dot"))
if input_port == 0 and output_port == 0:
return False
else:
# For other combinations of i/o, we will return
# "None", i.e. "I don't know."
return None
if __name__ == '__main__':
# simulate quadrotor w/ LQR controller using forward Euler integration.
# fixed point
xd = np.zeros(n)
xd[0:2] = [2, 1]
ud = np.zeros(m)
ud[:] = mass * g / 4
x_u = np.hstack((xd, ud))
partials = jacobian(CalcF, x_u)
A0 = partials[:, 0:n]
B0 = partials[:, n:n + m]
Q = 10 * np.eye(n)
R = np.eye(m)
# get LQR controller about the fixed point
K0, S0 = LinearQuadraticRegulator(A0, B0, Q, R)
# simulate stabilizing about fixed point using LQR controller
dt = 0.001
N = int(4.0 / dt)
x = np.zeros((N + 1, n))
x0 = np.zeros(n)
x[0] = x0
#print CalcFx(x_u)
#print CalcFu(x_u)
#print jacobian(CalcF, x_u)
#%% simulate and plot
dt = 0.001
T = 20000
t = dt*np.arange(T+1)
x = np.zeros((T+1, 2))
x[0] = [0, 0.1]
# desired fixed point
xd = np.array([-np.pi, 0])
# linearize about upright fixed point
f_x_u = jacobian(CalcF, np.hstack((xd, [0])))
A0 = f_x_u[:, 0:2]
B0 = f_x_u[:, 2:3]
K0, S0 = LinearQuadraticRegulator(A0, B0, 10*np.diag([1,1]), 1*np.eye(1))
for i in range(T):
x_u = np.hstack((x[i], Tau(x[i])))
x[i+1] = x[i] + dt*CalcF(x_u)
fig = plt.figure(figsize=(6,12), dpi = 100)
ax_x = fig.add_subplot(311)
ax_x.set_ylabel("x")
ax_x.plot(t, x[:,0])
ax_x.axhline(np.pi, color='r', ls='--')
def test_kinematics_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# Ensure mismatched sizes throw an error.
q_bad = np.zeros(num_q + 1)
v_bad = np.zeros(num_v + 1)
bad_args_list = (
(q_bad, ),
(q_bad, v),
(q, v_bad),
)
for bad_args in bad_args_list:
with self.assertRaises(SystemExit):
tree.doKinematics(*bad_args)
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Relative RPY, checking autodiff (#9886).
q[:] = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
q_ad = np.array([AutoDiffXd(x) for x in q])
world = tree.findFrame("world")
frame = tree.findFrame("arm_com")
kinsol = tree.doKinematics(q)
rpy = tree.relativeRollPitchYaw(
cache=kinsol,
from_body_or_frame_ind=world.get_frame_index(),
to_body_or_frame_ind=frame.get_frame_index())
kinsol_ad = tree.doKinematics(q_ad)
rpy_ad = tree.relativeRollPitchYaw(
cache=kinsol_ad,
from_body_or_frame_ind=world.get_frame_index(),
to_body_or_frame_ind=frame.get_frame_index())
for x, x_ad in zip(rpy, rpy_ad):
self.assertEqual(x, x_ad.value())
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Geometric Jacobian.
# Construct a new tree with a quaternion floating base.
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"),
floating_base_type=FloatingBaseType.kQuaternion)
num_q = 8
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = tree.getZeroConfiguration()
v = np.zeros(num_v)
kinsol = tree.doKinematics(q, v)
# - Sanity check sizes.
J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0)
J_eeDotTimesV = tree.geometricJacobianDotTimesV(kinsol, 0, 2, 0)
self.assertEqual(J_default.shape[0], 6)
self.assertEqual(J_default.shape[1], num_v)
self.assertEqual(len(v_indices_default), num_v)
self.assertEqual(J_eeDotTimesV.shape[0], 6)
# - Check QDotToVelocity and VelocityToQDot methods
q = tree.getZeroConfiguration()
v_real = np.zeros(num_v)
q_ad = np.array(list(map(AutoDiffXd, q)))
v_real_ad = np.array(list(map(AutoDiffXd, v_real)))
kinsol = tree.doKinematics(q)
kinsol_ad = tree.doKinematics(q_ad)
qd = tree.transformVelocityToQDot(kinsol, v_real)
v = tree.transformQDotToVelocity(kinsol, qd)
qd_ad = tree.transformVelocityToQDot(kinsol_ad, v_real_ad)
v_ad = tree.transformQDotToVelocity(kinsol_ad, qd_ad)
self.assertEqual(qd.shape, (num_q, ))
self.assertEqual(v.shape, (num_v, ))
self.assertEqual(qd_ad.shape, (num_q, ))
self.assertEqual(v_ad.shape, (num_v, ))
v_to_qdot = tree.GetVelocityToQDotMapping(kinsol)
qdot_to_v = tree.GetQDotToVelocityMapping(kinsol)
v_to_qdot_ad = tree.GetVelocityToQDotMapping(kinsol_ad)
qdot_to_v_ad = tree.GetQDotToVelocityMapping(kinsol_ad)
self.assertEqual(v_to_qdot.shape, (num_q, num_v))
self.assertEqual(qdot_to_v.shape, (num_v, num_q))
self.assertEqual(v_to_qdot_ad.shape, (num_q, num_v))
self.assertEqual(qdot_to_v_ad.shape, (num_v, num_q))
v_map = tree.transformVelocityMappingToQDotMapping(
kinsol, np.eye(num_v))
qd_map = tree.transformQDotMappingToVelocityMapping(
kinsol, np.eye(num_q))
v_map_ad = tree.transformVelocityMappingToQDotMapping(
kinsol_ad, np.eye(num_v))
qd_map_ad = tree.transformQDotMappingToVelocityMapping(
kinsol_ad, np.eye(num_q))
self.assertEqual(v_map.shape, (num_v, num_q))
self.assertEqual(qd_map.shape, (num_q, num_v))
self.assertEqual(v_map_ad.shape, (num_v, num_q))
self.assertEqual(qd_map_ad.shape, (num_q, num_v))
# - Check FindBody and FindBodyIndex methods
body_name = "arm"
body = tree.FindBody(body_name)
self.assertEqual(body.get_body_index(), tree.FindBodyIndex(body_name))
# - Check ChildOfJoint methods
body = tree.FindChildBodyOfJoint("theta")
self.assertIsInstance(body, RigidBody)
self.assertEqual(body.get_name(), "arm")
self.assertEqual(tree.FindIndexOfChildBodyOfJoint("theta"), 2)
# - Check that default value for in_terms_of_qdot is false.
J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, False)
self.assertTrue((J_default == J_not_in_terms_of_q_dot).all())
self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot)
# - Check with in_terms_of_qdot set to True.
J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, True)
self.assertEqual(J_in_terms_of_q_dot.shape[0], 6)
self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q)
self.assertEqual(len(v_indices_in_terms_of_qdot), num_q)
# - Check transformPointsJacobian methods
q = tree.getZeroConfiguration()
v = np.zeros(num_v)
kinsol = tree.doKinematics(q, v)
Jv = tree.transformPointsJacobian(cache=kinsol,
points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1,
in_terms_of_qdot=False)
Jq = tree.transformPointsJacobian(cache=kinsol,
points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1,
in_terms_of_qdot=True)
JdotV = tree.transformPointsJacobianDotTimesV(cache=kinsol,
points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1)
self.assertEqual(Jv.shape, (3, num_v))
self.assertEqual(Jq.shape, (3, num_q))
self.assertEqual(JdotV.shape, (3, ))
# - Check that drawKinematicTree runs
tree.drawKinematicTree(
join(os.environ["TEST_TMPDIR"], "test_graph.dot"))
# - Check relative twist method
twist = tree.relativeTwist(cache=kinsol,
base_or_frame_ind=0,
body_or_frame_ind=1,
expressed_in_body_or_frame_ind=0)
self.assertEqual(twist.shape[0], 6)
from pydrake.forwarddiff import jacobian
import numpy as np
from numpy import sin,cos
def f(x):
return np.array([x[0] + x[1]])
x = np.array([0,1])
fx = jacobian(f,x)
print fx
