def get_external_force(self, body_index, force):
# print(f"Force {force[:3]}")
external_force = ExternallyAppliedSpatialForce()
external_force.body_index = body_index
external_force.p_BoBq_B = np.zeros(3)
external_force.F_Bq_W = SpatialForce(tau=force[0:3], f=force[3:6])
return external_force
def controller(
X_TPdes: RigidTransform,
V_TPdes: SpatialVelocity,
x: VectorArg(nx),
) -> ForceList:
# Cribbed from:
# https://github.com/gizatt/drake_hydra_interact/tree/cce3ecbb
frame_W = plant.world_frame()
plant.SetPositionsAndVelocities(context, model_instance, x)
X_WT = plant.CalcRelativeTransform(context, frame_W, frame_T)
V_WT = me.get_frame_spatial_velocity(plant, context, frame_W, frame_T)
X_WPdes = X_WT @ X_TPdes
V_WPdes = V_WT.ComposeWithMovingFrameVelocity(
X_WT.translation(), V_TPdes.Rotate(X_WT.rotation()))
X_WP = plant.CalcRelativeTransform(context, frame_W, frame_P)
R_WP = X_WP.rotation()
V_WP = me.get_frame_spatial_velocity(plant, context, frame_W, frame_P)
# Transform to "negative error": desired w.r.t. actual,
# expressed in world frame (for applying the force).
p_PPdes_W = X_WPdes.translation() - X_WP.translation()
# N.B. We don't project away symmetry here because we're expecting
# smooth trajectories (for now).
R_PPdes = R_WP.inverse() @ X_WPdes.rotation()
axang3_PPdes = rotation_matrix_to_axang3(R_PPdes)
axang3_PPdes_W = R_WP @ axang3_PPdes
V_PPdes_W = V_WPdes - V_WP
# Compute wrench components.
f_P_W = Kp_xyz @ p_PPdes_W + Kd_xyz @ V_PPdes_W.translational()
tau_P_W = (Kp_rot(R_WP) @ axang3_PPdes_W +
Kd_rot(R_WP) @ V_PPdes_W.rotational())
F_P_W_feedback = SpatialForce(tau=tau_P_W, f=f_P_W)
# Add gravity-compensation term.
g_W = plant.gravity_field().gravity_vector()
F_Pcm_W = SpatialForce(tau=[0, 0, 0], f=-g_W * M_PPo_P.get_mass())
p_PoPcm_W = R_WP @ M_PPo_P.get_com()
F_P_W_feedforward = F_Pcm_W.Shift(-p_PoPcm_W)
# Package it up.
F_P_W = F_P_W_feedback + F_P_W_feedforward
external_force = ExternallyAppliedSpatialForce()
external_force.body_index = frame_P.body().index()
external_force.F_Bq_W = F_P_W
external_force.p_BoBq_B = (
frame_P.GetFixedPoseInBodyFrame().translation())
return ForceList([external_force])
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 DoCalcAbstractOutput(self, context, y_data):
self.callback_lock.acquire()
if self.selected_body and self.grab_in_progress:
# Simple inverse dynamics PD rule to drive object to desired pose.
body = self.selected_body
# Load in robot state
x_in = self.EvalVectorInput(context, 1).get_value()
self.mbp.SetPositionsAndVelocities(self.mbp_context, x_in)
TF_object = self.mbp.EvalBodyPoseInWorld(self.mbp_context, body)
xyz = TF_object.translation()
R = TF_object.rotation().matrix()
TFd_object = self.mbp.EvalBodySpatialVelocityInWorld(
self.mbp_context, body)
xyzd = TFd_object.translational()
Rd = TFd_object.rotational()
# Match the object position directly to the hydra position.
xyz_desired = self.desired_pose_in_world_frame.translation()
# Regress xyz back to just the hydra pose in the attraction case
if self.previously_freezing_rotation != self.freeze_rotation:
self.selected_body_init_offset = TF_object
self.grab_update_hydra_pose = RigidTransform(
self.desired_pose_in_world_frame)
self.previously_freezing_rotation = self.freeze_rotation
if self.freeze_rotation:
R_desired = self.selected_body_init_offset.rotation().matrix()
else:
# Figure out the relative rotation of the hydra from its initial posture
to_init_hydra_tf = self.grab_update_hydra_pose.inverse()
desired_delta_rotation = to_init_hydra_tf.multiply(
self.desired_pose_in_world_frame).matrix()[:3, :3]
# Transform the current object rotation into the init hydra frame, apply that relative tf, and
# then transform back
to_init_hydra_tf_rot = to_init_hydra_tf.matrix()[:3, :3]
R_desired = to_init_hydra_tf_rot.T.dot(
desired_delta_rotation.dot(
to_init_hydra_tf_rot.dot(
self.selected_body_init_offset.rotation().matrix())
))
# Could also pull the rotation back, but it's kind of nice to be able to recenter the object
# without messing up a randomized rotation.
#R_desired = (self.desired_pose_in_world_frame.rotation().matrix()*self.attract_factor +
# R_desired*(1.-self.attract_factor))
# Apply PD in cartesian space
xyz_e = xyz_desired - xyz
xyzd_e = -xyzd
f = 100. * xyz_e + 10. * xyzd_e
R_err_in_body_frame = np.linalg.inv(R).dot(R_desired)
aa = AngleAxis(R_err_in_body_frame)
tau_p = R.dot(aa.axis() * aa.angle())
tau_d = -Rd
tau = tau_p + 0.1 * tau_d
exerted_force = SpatialForce(tau=tau, f=f)
out = ExternallyAppliedSpatialForce()
out.F_Bq_W = exerted_force
out.body_index = self.selected_body.index()
y_data.set_value(VectorExternallyAppliedSpatialForced([out]))
else:
y_data.set_value(VectorExternallyAppliedSpatialForced([]))
self.callback_lock.release()