def ManipulatorDynamics(plant, q, v=None): if isinstance(plant, RigidBodyTree): # TODO(russt): Zap this branch once I'm finished porting to MBP. if v is None: v = np.zeros((plant.get_num_velocities(), 1)) kinsol = plant.doKinematics(q, v) M = plant.massMatrix(kinsol) tauG = -plant.dynamicsBiasTerm(kinsol, {}, False) Cv = plant.dynamicsBiasTerm(kinsol, {}, True) + tauG B = plant.B tauExt = 0 * tauG else: # MultibodyPlant version assert (isinstance(plant, MultibodyPlant)) context = plant.CreateDefaultContext() plant.SetPositions(context, q) if v is not None: plant.SetVelocities(context, v) M = plant.CalcMassMatrixViaInverseDynamics(context) Cv = plant.CalcBiasTerm(context) tauG = plant.CalcGravityGeneralizedForces(context) B = plant.MakeActuationMatrix() forces = MultibodyForces(plant) plant.CalcForceElementsContribution(context, forces) tauExt = forces.generalized_forces() return (M, Cv, tauG, B, tauExt)
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() H = plant.CalcMassMatrixViaInverseDynamics(context) Cv = plant.CalcBiasTerm(context) self.assertTrue(H.shape == (2, 2)) self.assert_sane(H) self.assertTrue(Cv.shape == (2, )) self.assert_sane(Cv, nonzero=False) nv = plant.num_velocities() vd_d = np.zeros(nv) tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant)) self.assertEqual(tau.shape, (2, )) self.assert_sane(tau, nonzero=False) # - Existence checks. self.assertEqual(plant.CalcPotentialEnergy(context), 0) plant.CalcConservativePower(context) tau_g = plant.CalcGravityGeneralizedForces(context) self.assertEqual(tau_g.shape, (nv, )) self.assert_sane(tau_g, nonzero=False) forces = MultibodyForces(plant=plant) plant.CalcForceElementsContribution(context=context, forces=forces)
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) # Getting ready for when we set foot on Mars :-). plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71])) plant.Finalize() context = plant.CreateDefaultContext() # Set an arbitrary configuration away from the model's fixed point. plant.SetPositions(context, [0.1, 0.2]) H = plant.CalcMassMatrixViaInverseDynamics(context) Cv = plant.CalcBiasTerm(context) self.assertTrue(H.shape == (2, 2)) self.assert_sane(H) self.assertTrue(Cv.shape == (2, )) self.assert_sane(Cv, nonzero=False) nv = plant.num_velocities() vd_d = np.zeros(nv) tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant)) self.assertEqual(tau.shape, (2, )) self.assert_sane(tau, nonzero=False) # - Existence checks. # Gravity leads to non-zero potential energy. self.assertNotEqual(plant.CalcPotentialEnergy(context), 0) plant.CalcConservativePower(context) tau_g = plant.CalcGravityGeneralizedForces(context) self.assertEqual(tau_g.shape, (nv, )) self.assert_sane(tau_g, nonzero=True) forces = MultibodyForces(plant=plant) plant.CalcForceElementsContribution(context=context, forces=forces)
def ManipulatorDynamics(plant, q, v=None): if isinstance(plant, RigidBodyTree): # TODO(russt): Zap this branch once I'm finished porting to MBP. if v is None: v = np.zeros((plant.get_num_velocities(),1)) kinsol = plant.doKinematics(q,v) M = plant.massMatrix(kinsol) tauG = -plant.dynamicsBiasTerm(kinsol, {}, False) Cv = plant.dynamicsBiasTerm(kinsol, {}, True) + tauG B = plant.B tauExt = 0*tauG else: # MultibodyPlant version assert(isinstance(plant, MultibodyPlant)) context = plant.CreateDefaultContext() plant.SetPositions(context, q) if v is not None: plant.SetVelocities(context, v) M = plant.CalcMassMatrixViaInverseDynamics(context) Cv = plant.CalcBiasTerm(context) tauG = plant.CalcGravityGeneralizedForces(context) B = plant.MakeActuationMatrix() forces = MultibodyForces(plant) plant.CalcForceElementsContribution(context, forces) tauExt = forces.generalized_forces() return (M, Cv, tauG, B, tauExt)
def test_deprecated_tree_api(self): plant = MultibodyPlant() plant.Finalize() with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) num_expected_warnings = [0] def expect_new_warning(msg_part): num_expected_warnings[0] += 1 self.assertEqual(len(w), num_expected_warnings[0]) self.assertIn(msg_part, str(w[-1].message)) tree = plant.tree() expect_new_warning("`tree()`") MobilizerIndex(0) expect_new_warning("`MobilizerIndex`") BodyNodeIndex(0) expect_new_warning("`BodyNodeIndex`") MultibodyForces(model=tree) expect_new_warning("`MultibodyForces(plant)`") element = plant.world_body() self.assertIsInstance(element.get_parent_tree(), MultibodyTree) expect_new_warning("`get_parent_tree()`") # Check old spellings (no deprecation warnings). self.check_old_spelling_exists(tree.CalcRelativeTransform) self.check_old_spelling_exists(tree.CalcPointsPositions) self.check_old_spelling_exists( tree.CalcFrameGeometricJacobianExpressedInWorld) self.check_old_spelling_exists(tree.EvalBodyPoseInWorld) self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow) self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow) self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld) self.check_old_spelling_exists(tree.GetPositionsFromArray) self.check_old_spelling_exists(tree.GetVelocitiesFromArray) self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics) self.check_old_spelling_exists(tree.CalcBiasTerm) self.check_old_spelling_exists(tree.CalcInverseDynamics) self.check_old_spelling_exists(tree.num_frames) self.check_old_spelling_exists(tree.get_body) self.check_old_spelling_exists(tree.get_joint) self.check_old_spelling_exists(tree.get_joint_actuator) self.check_old_spelling_exists(tree.get_frame) self.check_old_spelling_exists(tree.GetModelInstanceName) context = plant.CreateDefaultContext() # All body poses. X_WB, = tree.CalcAllBodyPosesInWorld(context) self.assertIsInstance(X_WB, Isometry3) v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertIsInstance(v_WB, SpatialVelocity)
def test_inverse_dynamics_controller(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) Parser(plant).AddModelFromFile(sdf_path) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.mutable_gravity_field().set_gravity_vector([0.0, 0.0, 0.0]) plant.Finalize() # We verify the (known) size of the model. kNumPositions = 7 kNumVelocities = 7 kNumActuators = 7 kStateSize = kNumPositions + kNumVelocities self.assertEqual(plant.num_positions(), kNumPositions) self.assertEqual(plant.num_velocities(), kNumVelocities) self.assertEqual(plant.num_actuators(), kNumActuators) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = InverseDynamicsController(robot=plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = controller.get_input_port(0) desired_state_port = controller.get_input_port(1) desired_acceleration_port = controller.get_input_port(2) control_port = controller.get_output_port(0) self.assertEqual(desired_acceleration_port.size(), kNumVelocities) self.assertEqual(estimated_state_port.size(), kStateSize) self.assertEqual(desired_state_port.size(), kStateSize) self.assertEqual(control_port.size(), kNumVelocities) # Current state. q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # Reference state and acceleration. q_r = q + 0.1*np.ones_like(q) v_r = v + 0.1*np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term estimated_state_port.FixValue(context, x) desired_state_port.FixValue(context, x_r) desired_acceleration_port.FixValue(context, vd_r) controller.set_integral_value(context, integral_term) # Set the plant's context. plant_context = plant.CreateDefaultContext() x_plant = plant.GetMutablePositionsAndVelocities(plant_context) x_plant[:] = x # Compute the expected value of the generalized forces using # inverse dynamics. tau_id = plant.CalcInverseDynamics( plant_context, vd_d, MultibodyForces(plant)) # Verify the result. controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) # Getting ready for when we set foot on Mars :-). gravity_vector = np.array([0.0, 0.0, -3.71]) plant.mutable_gravity_field().set_gravity_vector(gravity_vector) np.testing.assert_equal(plant.gravity_field().gravity_vector(), gravity_vector) plant.Finalize() context = plant.CreateDefaultContext() # Set an arbitrary configuration away from the model's fixed point. plant.SetPositions(context, [0.1, 0.2]) M = plant.CalcMassMatrixViaInverseDynamics(context) Cv = plant.CalcBiasTerm(context) self.assertTrue(M.shape == (2, 2)) self.assert_sane(M) self.assertTrue(Cv.shape == (2, )) self.assert_sane(Cv, nonzero=False) nv = plant.num_velocities() vd_d = np.zeros(nv) tau = plant.CalcInverseDynamics( context, vd_d, MultibodyForces(plant)) self.assertEqual(tau.shape, (2,)) self.assert_sane(tau, nonzero=False) # - Existence checks. # Gravity leads to non-zero potential energy. self.assertNotEqual(plant.CalcPotentialEnergy(context), 0) plant.CalcConservativePower(context) tau_g = plant.CalcGravityGeneralizedForces(context) self.assertEqual(tau_g.shape, (nv,)) self.assert_sane(tau_g, nonzero=True) B = plant.MakeActuationMatrix() np.testing.assert_equal(B, np.array([[0.], [1.]])) forces = MultibodyForces(plant=plant) plant.CalcForceElementsContribution(context=context, forces=forces) # Test generalized forces. forces.mutable_generalized_forces()[:] = 1 np.testing.assert_equal(forces.generalized_forces(), 1) forces.SetZero() np.testing.assert_equal(forces.generalized_forces(), 0) # Test body force accessors and mutators. link2 = plant.GetBodyByName("Link2") self.assertIsInstance( link2.GetForceInWorld(context, forces), SpatialForce) forces.SetZero() F_expected = np.array([1, 2, 3, 4, 5, 6]) link2.AddInForceInWorld( context, F_Bo_W=SpatialForce(F=F_expected), forces=forces) np.testing.assert_equal( link2.GetForceInWorld(context, forces).get_coeffs(), F_expected) link2.AddInForce( context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected), frame_E=plant.world_frame(), forces=forces) # Also check accumulation. np.testing.assert_equal( link2.GetForceInWorld(context, forces).get_coeffs(), 2 * F_expected)
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) # Getting ready for when we set foot on Mars :-). gravity_vector = np.array([0.0, 0.0, -3.71]) plant.mutable_gravity_field().set_gravity_vector(gravity_vector) np.testing.assert_equal(plant.gravity_field().gravity_vector(), gravity_vector) plant.Finalize() context = plant.CreateDefaultContext() # Set an arbitrary configuration away from the model's fixed point. plant.SetPositions(context, [0.1, 0.2]) M = plant.CalcMassMatrixViaInverseDynamics(context) Cv = plant.CalcBiasTerm(context) self.assertTrue(M.shape == (2, 2)) self.assert_sane(M) self.assertTrue(Cv.shape == (2, )) self.assert_sane(Cv, nonzero=False) nv = plant.num_velocities() vd_d = np.zeros(nv) tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant)) self.assertEqual(tau.shape, (2, )) self.assert_sane(tau, nonzero=False) # - Existence checks. # Gravity leads to non-zero potential energy. self.assertNotEqual(plant.CalcPotentialEnergy(context), 0) plant.CalcConservativePower(context) tau_g = plant.CalcGravityGeneralizedForces(context) self.assertEqual(tau_g.shape, (nv, )) self.assert_sane(tau_g, nonzero=True) B = plant.MakeActuationMatrix() np.testing.assert_equal(B, np.array([[0.], [1.]])) forces = MultibodyForces(plant=plant) plant.CalcForceElementsContribution(context=context, forces=forces) # Test generalized forces. forces.mutable_generalized_forces()[:] = 1 np.testing.assert_equal(forces.generalized_forces(), 1) forces.SetZero() np.testing.assert_equal(forces.generalized_forces(), 0) # Test body force accessors and mutators. link2 = plant.GetBodyByName("Link2") self.assertIsInstance(link2.GetForceInWorld(context, forces), SpatialForce) forces.SetZero() F_expected = np.array([1, 2, 3, 4, 5, 6]) link2.AddInForceInWorld(context, F_Bo_W=SpatialForce(F=F_expected), forces=forces) np.testing.assert_equal( link2.GetForceInWorld(context, forces).get_coeffs(), F_expected) link2.AddInForce(context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected), frame_E=plant.world_frame(), forces=forces) # Also check accumulation. np.testing.assert_equal( link2.GetForceInWorld(context, forces).get_coeffs(), 2 * F_expected)
print("Acrobot bias term at q = ", q, "and v = ", v, "is Cv = ") print(Cv) # We can separately get the gravitational effects N = plant.CalcGravityGeneralizedForces(context) print("Acrobot gravitational generalized forces at q = ", q, " is N = ") print(N) # Evaluating the controls nU = plant.num_actuated_dofs() nA = plant.num_actuators() print('Acrobot has ', nU, ' actuated joint(s) and ', nA, ' actuator(s)') # We can get the actuation matrix, a permutation matrix as: B = plant.MakeActuationMatrix() print('The acutator selection matrix for acrobot is B = ') print(B) # Note that calculating the dynamics in this fashion is not computationally efficient. It would be more efficient to use plant.CalcInverseDynamics instead, given the generalized acceleration and applied forces # Create empty generalized applied forces forces = MultibodyForces(plant) forces.SetZero() # Create some generalized accelerations dv = [0.2, 0.6] # Do inverse dynamics to find the generalized forces needed to achieve these accelerations # NOTE: INVERSE DYNAMICS DOES NOT AUTOMATICALLY ENCODE THE GRAVITATIONAL FORCES tau = plant.CalcInverseDynamics(context, dv, forces) print(f"Inverse dynamics without gravity f = {tau}") # To encode generalized forces - including gravity - we can add them in after the fact, or add them in to the MultibodyForces force = forces.mutable_generalized_forces() force[:] = N tau_2 = plant.CalcInverseDynamics(context, dv, forces) print(f"Inverse dynamics with gravity f = {tau_2}")