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