diff --git a/drake/multibody/rigid_body_plant/BUILD.bazel b/drake/multibody/rigid_body_plant/BUILD.bazel index 7fdac76f0236..997846c4a05a 100644 --- a/drake/multibody/rigid_body_plant/BUILD.bazel +++ b/drake/multibody/rigid_body_plant/BUILD.bazel @@ -68,7 +68,9 @@ drake_cc_library( ":compliant_contact_model", "//drake/common:copyable_unique_ptr", "//drake/common:essential", + "//drake/math:orthonormal_basis", "//drake/multibody:rigid_body_tree", + "//drake/multibody/constraint:constraint_solver", "//drake/solvers:mathematical_program", "//drake/systems/framework", ], diff --git a/drake/multibody/rigid_body_plant/compliant_contact_model.h b/drake/multibody/rigid_body_plant/compliant_contact_model.h index f83f641bfb92..398f309d2593 100644 --- a/drake/multibody/rigid_body_plant/compliant_contact_model.h +++ b/drake/multibody/rigid_body_plant/compliant_contact_model.h @@ -62,6 +62,20 @@ class CompliantContactModel { /// aborts. (See CompliantContactParameters for details on valid ranges.) void set_model_parameters(const CompliantContactModelParameters& values); + /// Given two collision elements (with their own defined compliant material + /// properties, computes the _derived_ parameters for the _contact_. Returns + /// the portion of the squish attributable to Element `a` (sₐ). Element `b`'s + /// squish factor is simply 1 - sₐ. See contact_model_doxygen.h for details. + /// @param[in] a The first element in the contact. + /// @param[in] b The second element in the contact. + /// @param[out] parameters The net _contact_ parameters. + /// @retval sₐ The "squish" factor of Element `a` -- the fraction of the full + /// penetration deformation that `a` experiences. + double CalcContactParameters( + const multibody::collision::Element& a, + const multibody::collision::Element& b, + CompliantMaterial* parameters) const; + private: // Computes the friction coefficient based on the relative tangential // *speed* of the contact point on A relative to B (expressed in B), v_BAc. @@ -75,20 +89,6 @@ class CompliantContactModel { // range [0, 1] where the f''(0) = f''(1) = f'(0) = f'(1) = 0. static T step5(const T& x); - // Given two collision elements (with their own defined compliant material - // properties, computes the _derived_ parameters for the _contact_. Returns - // The portion of the squish attributable to Element `a` (sₐ). Element `b`'s - // squish factor is simply 1 - sₐ. See contact_model_doxygen.h for details. - // @param[in] a The first element in the contact. - // @param[in] b The second element in the contact. - // @param[out] parameters The net _contact_ parameters. - // @retval sₐ The "squish" factor of Element `a` -- the fraction of the full - // penetration deformation that `a` experiences. - double CalcContactParameters( - const multibody::collision::Element& a, - const multibody::collision::Element& b, - CompliantMaterial* parameters) const; - // Note: this is the *inverse* of the v_stiction_tolerance parameter to // optimize for the division. double inv_v_stiction_tolerance_{ diff --git a/drake/multibody/rigid_body_plant/rigid_body_plant.cc b/drake/multibody/rigid_body_plant/rigid_body_plant.cc index 70c36c037e7e..1ab39b38f026 100644 --- a/drake/multibody/rigid_body_plant/rigid_body_plant.cc +++ b/drake/multibody/rigid_body_plant/rigid_body_plant.cc @@ -8,6 +8,7 @@ #include "drake/common/autodiff.h" #include "drake/common/drake_assert.h" #include "drake/common/eigen_types.h" +#include "drake/math/orthonormal_basis.h" #include "drake/multibody/kinematics_cache.h" #include "drake/multibody/rigid_body_plant/compliant_contact_model.h" #include "drake/multibody/rigid_body_plant/compliant_material.h" @@ -50,6 +51,10 @@ RigidBodyPlant::RigidBodyPlant(std::unique_ptr> tree, // Declares an abstract valued output port for contact information. contact_output_port_index_ = DeclareContactResultsOutputPort(); + + // Schedule time stepping update. + if (timestep > 0.0) + this->DeclarePeriodicDiscreteUpdate(timestep); } template @@ -419,6 +424,346 @@ void RigidBodyPlant::CalcKinematicsResultsOutput( kinematics_results->UpdateFromContext(context); } +// Computes the stiffness, damping, and friction coefficient for a contact. +template +void RigidBodyPlant::CalcContactStiffnessDampingMuAndNumHalfConeEdges( + const drake::multibody::collision::PointPair& contact, + double* stiffness, + double* damping, + double* mu, + int* num_half_cone_edges) const { + DRAKE_DEMAND(stiffness); + DRAKE_DEMAND(damping); + DRAKE_DEMAND(mu); + DRAKE_DEMAND(num_half_cone_edges); + + // Get the compliant material parameters. + CompliantMaterial material; + compliant_contact_model_->CalcContactParameters( + *contact.elementA, *contact.elementB, &material); + + // Get the stiffness. Young's modulus is force per area, while stiffness is + // force per length. That means that we must multiply by a characteristic + // radius. See @ref hunt_crossley (in contact_model_doxygen.h) for a lengthy + // discussion on converting Young's Modulus to a stiffness. + // The "length" will be incorporated using the contact depth. + // TODO(edrumwri): Make characteristic radius user settable. + const double characteristic_radius = 1e-2; // 1 cm. + *stiffness = material.youngs_modulus() * characteristic_radius; + + // Get the damping value (b) from the compliant model dissipation (α). + // Equation (16) from [Hunt 1975] yields b = 3/2 * α * k * x. We can assume + // that the stiffness and dissipation are predicated upon small deformation + // (x). Put another way, we determine the damping coefficient for a harmonic + // oscillator from linearizing the dissipation factor about the characteristic + // deformation; the system will behave like a harmonic oscillator oscillating + // about x = characteristic_deformation (in meters). + // TODO(edrumwri): Make characteristic deformation user settable. + const double characteristic_deformation = 1e-4; // 1 mm + *damping = material.dissipation() * 1.5 * (*stiffness) * + characteristic_deformation; + + // Get the coefficient of friction. + *mu = material.static_friction(); + + // TODO(edrumwri): The number of half-cone edges should be able to be set on + // a per-geometry pair basis. For now, just set the value to pyramidal + // friction. + *num_half_cone_edges = 2; + + // Verify the friction directions are set correctly. + DRAKE_DEMAND(*num_half_cone_edges >= 2); +} + +// Gets A's translational velocity relative to B's translational velocity at a +// point common to the two rigid bodies. +// @param p_W The point of contact (defined in the world frame). +// @returns the relative velocity at p_W expressed in the world frame. +template +Vector3 RigidBodyPlant::CalcRelTranslationalVelocity( + const KinematicsCache& kinematics_cache, int body_a_index, + int body_b_index, const Vector3& p_W) const { + const auto& tree = this->get_rigid_body_tree(); + + // TODO(edrumwri): Convert this method to avoid Jacobian computation using + // RigidBodyTree::CalcBodySpatialVelocityInWorldFrame(). + + // The contact point in A's frame. + const auto X_AW = kinematics_cache.get_element(body_a_index) + .transform_to_world.inverse(Eigen::Isometry); + const Vector3 p_A = X_AW * p_W; + + // The contact point in B's frame. + const auto X_BW = kinematics_cache.get_element(body_b_index) + .transform_to_world.inverse(Eigen::Isometry); + const Vector3 p_B = X_BW * p_W; + + // Get the Jacobian matrices. + const auto JA = + tree.transformPointsJacobian(kinematics_cache, p_A, body_a_index, 0, + false); + const auto JB = + tree.transformPointsJacobian(kinematics_cache, p_B, body_b_index, 0, + false); + + // Compute the relative velocity in the world frame. + return (JA - JB) * kinematics_cache.getV(); +} + +// Updates a generalized force from a force of f (expressed in the world frame) +// applied at point p_W (defined in the global frame). +template +void RigidBodyPlant::UpdateGeneralizedForce( + const KinematicsCache& kinematics_cache, int body_a_index, + int body_b_index, const Vector3& p_W, const Vector3& f, + VectorX* gf) const { + const auto& tree = this->get_rigid_body_tree(); + + // TODO(edrumwri): Convert this method to avoid Jacobian computation using + // RigidBodyTree::dynamicsBiasTerm(). + + // The contact point in A's frame. + const auto X_AW = kinematics_cache.get_element(body_a_index) + .transform_to_world.inverse(Eigen::Isometry); + const Vector3 p_A = X_AW * p_W; + + // The contact point in B's frame. + const auto X_BW = kinematics_cache.get_element(body_b_index) + .transform_to_world.inverse(Eigen::Isometry); + const Vector3 p_B = X_BW * p_W; + + // Get the Jacobian matrices. + const auto JA = + tree.transformPointsJacobian(kinematics_cache, p_A, body_a_index, 0, + false); + const auto JB = + tree.transformPointsJacobian(kinematics_cache, p_B, body_b_index, 0, + false); + + // Compute the Jacobian transpose times the force, and use it to update gf. + (*gf) += (JA - JB).transpose() * f; +} + +// Evaluates the relative velocities between two bodies projected along the +// contact normals. +template +VectorX RigidBodyPlant::ContactNormalJacobianMult( + const std::vector& contacts, + const VectorX& q, const VectorX& v) const { + const auto& tree = this->get_rigid_body_tree(); + auto kinematics_cache = tree.doKinematics(q, v); + + // Create a result vector. + VectorX result(contacts.size()); + + // Loop through all contacts. + for (int i = 0; static_cast(i) < contacts.size(); ++i) { + // Get the two body indices. + const int body_a_index = contacts[i].elementA->get_body()->get_body_index(); + const int body_b_index = contacts[i].elementB->get_body()->get_body_index(); + + // The reported point on A's surface (As) in the world frame (W). + const Vector3 p_WAs = + kinematics_cache.get_element(body_a_index).transform_to_world * + contacts[i].ptA; + + // The reported point on B's surface (Bs) in the world frame (W). + const Vector3 p_WBs = + kinematics_cache.get_element(body_b_index).transform_to_world * + contacts[i].ptB; + + // Get the point of contact in the world frame. + const Vector3 p_W = (p_WAs + p_WBs) * 0.5; + + // The *relative* velocity of the contact point in A relative to that in + // B. + const auto v_W = CalcRelTranslationalVelocity(kinematics_cache, + body_a_index, body_b_index, p_W); + + // Get the projected normal velocity. + result[i] = v_W.dot(contacts[i].normal); + } + + return result; +} + +// Applies forces along the contact normals at the contact points and gets the +// effect out on the generalized forces. +template +VectorX RigidBodyPlant::TransposedContactNormalJacobianMult( + const std::vector& contacts, + const KinematicsCache& kinematics_cache, + const VectorX& f) const { + // Create a result vector. + VectorX result = VectorX::Zero(kinematics_cache.getV().size()); + + // Loop through all contacts. + for (int i = 0; static_cast(i) < contacts.size(); ++i) { + // Get the two body indices. + const int body_a_index = contacts[i].elementA->get_body()->get_body_index(); + const int body_b_index = contacts[i].elementB->get_body()->get_body_index(); + + // The reported point on A's surface (As) in the world frame (W). + const Vector3 p_WAs = + kinematics_cache.get_element(body_a_index).transform_to_world * + contacts[i].ptA; + + // The reported point on B's surface (Bs) in the world frame (W). + const Vector3 p_WBs = + kinematics_cache.get_element(body_b_index).transform_to_world * + contacts[i].ptB; + + // Get the point of contact in the world frame. + const Vector3 p_W = (p_WAs + p_WBs) * 0.5; + + // Get the contribution to the generalized force from a force of the + // specified normal applied at this point. + UpdateGeneralizedForce(kinematics_cache, body_a_index, body_b_index, p_W, + contacts[i].normal * f[i], &result); + } + + return result; +} + +// Evaluates the relative velocities between two bodies projected along the +// contact tangent directions. +template +VectorX RigidBodyPlant::ContactTangentJacobianMult( + const std::vector& contacts, + const VectorX& q, const VectorX& v, + const std::vector& half_num_cone_edges) const { + using std::cos; + using std::sin; + std::vector> basis_vecs; + const auto& tree = this->get_rigid_body_tree(); + auto kinematics_cache = tree.doKinematics(q, v); + + // Get the total (half) number of edges in the friction cones of all contacts. + const int total_edges = std::accumulate( + half_num_cone_edges.begin(), half_num_cone_edges.end(), 0); + + // Create a result vector. + VectorX result(total_edges); + + // Loop through all contacts. + for (int i = 0, result_index = 0; static_cast(i) < contacts.size(); + ++i) { + // Get the two body indices. + const int body_a_index = contacts[i].elementA->get_body()->get_body_index(); + const int body_b_index = contacts[i].elementB->get_body()->get_body_index(); + + // The reported point on A's surface (As) in the world frame (W). + const Vector3 p_WAs = + kinematics_cache.get_element(body_a_index).transform_to_world * + contacts[i].ptA; + + // The reported point on B's surface (Bs) in the world frame (W). + const Vector3 p_WBs = + kinematics_cache.get_element(body_b_index).transform_to_world * + contacts[i].ptB; + + // Get the point of contact in the world frame. + const Vector3 p_W = (p_WAs + p_WBs) * 0.5; + + // The *relative* velocity of the contact point in A relative to that in + // B. + const auto v_W = CalcRelTranslationalVelocity(kinematics_cache, + body_a_index, body_b_index, p_W); + + // Compute an orthonormal basis. + const int kXAxisIndex = 0, kYAxisIndex = 1, kZAxisIndex = 2; + auto R_WC = math::ComputeBasisFromAxis(kXAxisIndex, contacts[i].normal); + const Vector3 tan1_dir = R_WC.col(kYAxisIndex); + const Vector3 tan2_dir = R_WC.col(kZAxisIndex); + + // Set spanning tangent directions. + basis_vecs.resize(half_num_cone_edges[i]); + if (half_num_cone_edges[i] == 2) { + // Special case: pyramid friction. + basis_vecs.front() = tan1_dir; + basis_vecs.back() = tan2_dir; + } else { + for (int j = 0; j < half_num_cone_edges[i]; ++j) { + double theta = M_PI * j / + (static_cast(half_num_cone_edges[i]) - 1); + basis_vecs[j] = tan1_dir * cos(theta) + tan2_dir * sin(theta); + } + } + + // Loop over the spanning tangent directions. + for (int j = 0; j < static_cast(basis_vecs.size()); ++j) { + // Get the projected tangent velocity. + result[result_index++] = v_W.dot(basis_vecs[j]); + } + } + + return result; +} + +// Applies a force at the contact spanning directions at all contacts and gets +// the effect out on the generalized forces. +template +VectorX RigidBodyPlant::TransposedContactTangentJacobianMult( + const std::vector& contacts, + const KinematicsCache& kinematics_cache, + const VectorX& f, + const std::vector& half_num_cone_edges) const { + std::vector> basis_vecs; + + // Create a result vector. + VectorX result = VectorX::Zero(kinematics_cache.getV().size()); + + // Loop through all contacts. + for (int i = 0, result_index = 0; static_cast(i) < contacts.size(); + ++i) { + // Get the two body indices. + const int body_a_index = contacts[i].elementA->get_body()->get_body_index(); + const int body_b_index = contacts[i].elementB->get_body()->get_body_index(); + + // The reported point on A's surface (As) in the world frame (W). + const Vector3 p_WAs = + kinematics_cache.get_element(body_a_index).transform_to_world * + contacts[i].ptA; + + // The reported point on B's surface (Bs) in the world frame (W). + const Vector3 p_WBs = + kinematics_cache.get_element(body_b_index).transform_to_world * + contacts[i].ptB; + + // Get the point of contact in the world frame. + const Vector3 p_W = (p_WAs + p_WBs) * 0.5; + + // Compute an orthonormal basis. + const int kXAxisIndex = 0, kYAxisIndex = 1, kZAxisIndex = 2; + auto R_WC = math::ComputeBasisFromAxis(kXAxisIndex, contacts[i].normal); + const Vector3 tan1_dir = R_WC.col(kYAxisIndex); + const Vector3 tan2_dir = R_WC.col(kZAxisIndex); + + // Set spanning tangent directions. + basis_vecs.resize(half_num_cone_edges[i]); + if (half_num_cone_edges[i] == 2) { + // Special case: pyramid friction. + basis_vecs.front() = tan1_dir; + basis_vecs.back() = tan2_dir; + } else { + for (int j = 0; j < half_num_cone_edges[i]; ++j) { + double theta = M_PI * j / + (static_cast(half_num_cone_edges[i]) - 1); + basis_vecs[j] = tan1_dir * cos(theta) + tan2_dir * sin(theta); + } + } + + // Get the contribution to the generalized force from a force of the + // specified normal applied at this point. + for (int j = 0; j < static_cast(basis_vecs.size()); ++j) { + UpdateGeneralizedForce(kinematics_cache, body_a_index, body_b_index, p_W, + basis_vecs[j] * f[result_index++], &result); + } + } + + return result; +} + /* * TODO(hongkai.dai): This only works for templates on double, it does not * work for autodiff yet, I will add the code to compute the gradient of vdot @@ -537,29 +882,47 @@ void RigidBodyPlant::DoCalcDiscreteVariableUpdates( const drake::systems::Context& context, const std::vector*>&, drake::systems::DiscreteValues* updates) const { + using std::abs; + static_assert(std::is_same::value, "Only support templating on double for now"); // If plant state is continuous, no discrete state to update. if (!is_state_discrete()) return; - VectorX u = EvaluateActuatorInputs(context); + // Get the time step. + double dt = this->get_time_step(); + DRAKE_DEMAND(dt > 0.0); - auto x = context.get_discrete_state(0).get_value(); + VectorX u = this->EvaluateActuatorInputs(context); - const int nq = get_num_positions(); - const int nv = get_num_velocities(); - const int num_actuators = get_num_actuators(); + const int nq = this->get_num_positions(); + const int nv = this->get_num_velocities(); + const int num_actuators = this->get_num_actuators(); + // Initialize the velocity problem data. + drake::multibody::constraint::ConstraintVelProblemData data(nv); + + // Get the rigid body tree. + const auto& tree = this->get_rigid_body_tree(); + + // Get the system state. + auto x = context.get_discrete_state(0).get_value(); VectorX q = x.topRows(nq); VectorX v = x.bottomRows(nv); - auto kinsol = tree_->doKinematics(q, v); + auto kinematics_cache = tree.doKinematics(q, v); - drake::solvers::MathematicalProgram prog; - drake::solvers::VectorXDecisionVariable vn = - prog.NewContinuousVariables(nv, "vn"); + // Get the generalized inertia matrix and set up the inertia solve function. + auto H = tree.massMatrix(kinematics_cache); - auto H = tree_->massMatrix(kinsol); + // Compute the LDLT factorizations, which will be used by the solver. + Eigen::LDLT> ldlt(H); + DRAKE_DEMAND(ldlt.info() == Eigen::Success); + + // Set the inertia matrix solver. + data.solve_inertia = [&ldlt](const MatrixX& m) { + return ldlt.solve(m); + }; // There are no external wrenches, but it is a required argument in // dynamicsBiasTerm(). @@ -568,23 +931,210 @@ void RigidBodyPlant::DoCalcDiscreteVariableUpdates( // right_hand_side is the right hand side of the system's equations: // right_hand_side = B*u - C(q,v) VectorX right_hand_side = - -tree_->dynamicsBiasTerm(kinsol, no_external_wrenches); - if (num_actuators > 0) right_hand_side += tree_->B * u; - - // TODO(russt): Handle joint limits. - // TODO(russt): Handle contact constraints. - - // Add H*(vn - v)/h = right_hand_side - prog.AddLinearEqualityConstraint(H / timestep_, - H * v / timestep_ + right_hand_side, vn); + -tree.dynamicsBiasTerm(kinematics_cache, no_external_wrenches); + if (num_actuators > 0) right_hand_side += tree.B * u; + + // Determine the set of contact points corresponding to the current q. + std::vector contacts = + const_cast*>(&tree)->ComputeMaximumDepthCollisionPoints( + kinematics_cache, true); + + // Set the stabilization term for contact normal direction (kN). Also, + // determine the friction coefficients and (half) the number of friction cone + // edges. + data.gammaN.resize(contacts.size()); + data.kN.resize(contacts.size()); + data.mu.resize(contacts.size()); + data.r.resize(contacts.size()); + for (int i = 0; i < static_cast(contacts.size()); ++i) { + double stiffness, damping, mu; + int half_friction_cone_edges; + CalcContactStiffnessDampingMuAndNumHalfConeEdges( + contacts[i], &stiffness, &damping, &mu, &half_friction_cone_edges); + data.mu[i] = mu; + data.r[i] = half_friction_cone_edges; + + // Set cfm and erp parameters for contacts. + const double denom = dt * stiffness + damping; + const double cfm = 1.0 / denom; + const double erp = (dt * stiffness) / denom; + data.gammaN[i] = cfm; + data.kN[i] = erp * contacts[i].distance / dt; + } - prog.Solve(); - VectorX xn(get_num_states()); - const auto& vn_sol = prog.GetSolution(vn); + // Set the joint range of motion limits. + std::vector limits; + for (auto const& b : tree.bodies) { + if (!b->has_parent_body()) continue; + auto const& joint = b->getJoint(); + + // Joint limit forces are only implemented for single-axis joints. + if (joint.get_num_positions() == 1 && joint.get_num_velocities() == 1) { + const T qmin = joint.getJointLimitMin()(0); + const T qmax = joint.getJointLimitMax()(0); + DRAKE_DEMAND(qmin < qmax); + + // Get the current joint position and velocity. + const T& qjoint = q(b->get_position_start_index()); + const T& vjoint = v(b->get_velocity_start_index()); + + // See whether the joint is currently violated or the *current* joint + // velocity might lead to a limit violation. The latter is a heuristic to + // incorporate the joint limit into the time stepping calculations before + // it is violated. + if (qjoint < qmin || qjoint + vjoint * dt < qmin) { + // Institute a lower limit. + limits.push_back(JointLimit()); + limits.back().v_index = b->get_velocity_start_index(); + limits.back().signed_distance = (qjoint - qmin); + SPDLOG_DEBUG(drake::log(), "body name: {} ", b->get_name()); + SPDLOG_DEBUG(drake::log(), "joint name: {} ", joint.get_name()); + SPDLOG_DEBUG(drake::log(), "joint signed distance: {} ", + limits.back().signed_distance); + limits.back().lower_limit = true; + } + if (qjoint > qmax || qjoint + vjoint * dt > qmax) { + // Institute an upper limit. + limits.push_back(JointLimit()); + limits.back().v_index = b->get_velocity_start_index(); + limits.back().signed_distance = (qmax - qjoint); + SPDLOG_DEBUG(drake::log(), "body name: {} ", b->get_name()); + SPDLOG_DEBUG(drake::log(), "joint name: {} ", joint.get_name()); + SPDLOG_DEBUG(drake::log(), "joint signed distance: {} ", + limits.back().signed_distance); + limits.back().lower_limit = false; + } + } + } - // qn = q + h*qdn. - xn << q + timestep_ * tree_->transformVelocityToQDot(kinsol, vn_sol), vn_sol; + // Set up the N multiplication operator (projected velocity along the contact + // normals) and the N' multiplication operator (effect of contact normal + // forces on generalized forces). + data.N_mult = [this, &contacts, &q](const VectorX& w) -> VectorX { + return ContactNormalJacobianMult(contacts, q, w); + }; + data.N_transpose_mult = [this, &contacts, &kinematics_cache] + (const VectorX& f) -> VectorX { + return TransposedContactNormalJacobianMult(contacts, kinematics_cache, f); + }; + + // Set up the F multiplication operator (projected velocity along the contact + // tangent directions) and the F' multiplication operator (effect of contact + // frictional forces on generalized forces). + data.F_mult = [this, &contacts, &q, &data](const VectorX& w) -> + VectorX { + return ContactTangentJacobianMult(contacts, q, w, data.r); + }; + data.F_transpose_mult = [this, &contacts, &kinematics_cache, &data] + (const VectorX& f) -> VectorX { + return TransposedContactTangentJacobianMult(contacts, + kinematics_cache, f, data.r); + }; + + // Set the range-of-motion (L) Jacobian multiplication operator and the + // transpose_mult() operation. + data.L_mult = [this, &limits](const VectorX& w) -> VectorX { + VectorX result(limits.size()); + for (int i = 0; static_cast(i) < limits.size(); ++i) { + const int index = limits[i].v_index; + result[i] = (limits[i].lower_limit) ? w[index] : -w[index]; + } + return result; + }; + data.L_transpose_mult = [this, &v, &limits](const VectorX& lambda) { + VectorX result = VectorX::Zero(v.size()); + for (int i = 0; static_cast(i) < limits.size(); ++i) { + const int index = limits[i].v_index; + result[index] = (limits[i].lower_limit) ? lambda[i] : -lambda[i]; + } + return result; + }; + + // Output the Jacobians. + #ifdef SPDLOG_DEBUG_ON + MatrixX N(contacts.size(), v.size()), L(limits.size(), v.size()), + F(contacts.size() * 2, v.size()); + for (int i = 0; i < v.size(); ++i) { + VectorX unit = VectorX::Unit(v.size(), i); + N.col(i) = data.N_mult(unit); + F.col(i) = data.F_mult(unit); + L.col(i) = data.L_mult(unit); + } + SPDLOG_DEBUG(drake::log(), "N: {}", N); + SPDLOG_DEBUG(drake::log(), "F: {}", F); + SPDLOG_DEBUG(drake::log(), "L: {}", L); + #endif + + // Set the regularization and stabilization terms for contact tangent + // directions (kF). + const int total_friction_cone_edges = std::accumulate( + data.r.begin(), data.r.end(), 0); + data.kF.setZero(total_friction_cone_edges); + data.gammaF.setZero(total_friction_cone_edges); + data.gammaE.setZero(contacts.size()); + + // Set the regularization and stabilization terms for joint limit + // constraints (kL). + // TODO(edrumwri): Make cfm and erp individually settable. + const double default_limit_cfm = 1e-8; + const double default_limit_erp = 0.5; + data.kL.resize(limits.size()); + for (int i = 0; i < static_cast(limits.size()); ++i) + data.kL[i] = default_limit_erp * limits[i].signed_distance / dt; + data.gammaL.setOnes(limits.size()) *= default_limit_cfm; + + // Set Jacobians for bilateral constraint terms. + // TODO(edrumwri): Make erp individually settable. + const double default_bilateral_erp = 0.5; + data.kG = default_bilateral_erp * + tree.positionConstraints(kinematics_cache) / dt; + const auto G = tree.positionConstraintsJacobian(kinematics_cache, false); + data.G_mult = [this, &G](const VectorX& w) -> VectorX { + return G * w; + }; + data.G_transpose_mult = [this, &G](const VectorX& lambda) { + return G.transpose() * lambda; + }; + + // Integrate the forces into the momentum. + data.Mv = H * v + right_hand_side * dt; + + // Solve the rigid impact problem. + VectorX new_velocity, contact_force; + constraint_solver_.SolveImpactProblem(data, &contact_force); + constraint_solver_.ComputeGeneralizedVelocityChange(data, contact_force, + &new_velocity); + SPDLOG_DEBUG(drake::log(), "Actuator forces: {} ", u.transpose()); + SPDLOG_DEBUG(drake::log(), "Transformed actuator forces: {} ", + (tree.B * u).transpose()); + SPDLOG_DEBUG(drake::log(), "force: {}", right_hand_side.transpose()); + SPDLOG_DEBUG(drake::log(), "old velocity: {}", v.transpose()); + SPDLOG_DEBUG(drake::log(), "integrated forward velocity: {}", + data.solve_inertia(data.Mv).transpose()); + SPDLOG_DEBUG(drake::log(), "change in velocity: {}", + new_velocity.transpose()); + new_velocity += data.solve_inertia(data.Mv); + SPDLOG_DEBUG(drake::log(), "new velocity: {}", new_velocity.transpose()); + SPDLOG_DEBUG(drake::log(), "new configuration: {}", + (q + dt * tree.transformVelocityToQDot(kinematics_cache, new_velocity)). + transpose()); + SPDLOG_DEBUG(drake::log(), "N * new velocity: {} ", data.N_mult(new_velocity). + transpose()); + SPDLOG_DEBUG(drake::log(), "F * new velocity: {} ", data.F_mult(new_velocity). + transpose()); + SPDLOG_DEBUG(drake::log(), "L * new velocity: {} ", data.L_mult(new_velocity). + transpose()); + SPDLOG_DEBUG(drake::log(), "G * new velocity: {} ", data.G_mult(new_velocity). + transpose()); + SPDLOG_DEBUG(drake::log(), "G * v: {} ", data.G_mult(v).transpose()); + SPDLOG_DEBUG(drake::log(), "g(): {}", + tree.positionConstraints(kinematics_cache).transpose()); + + // qn = q + dt*qdot. + VectorX xn(this->get_num_states()); + xn << q + dt * tree.transformVelocityToQDot(kinematics_cache, new_velocity), + new_velocity; updates->get_mutable_vector(0).SetFromVector(xn); } diff --git a/drake/multibody/rigid_body_plant/rigid_body_plant.h b/drake/multibody/rigid_body_plant/rigid_body_plant.h index 0297788eda42..f13ec97f5530 100644 --- a/drake/multibody/rigid_body_plant/rigid_body_plant.h +++ b/drake/multibody/rigid_body_plant/rigid_body_plant.h @@ -8,6 +8,7 @@ #include #include "drake/common/drake_copyable.h" +#include "drake/multibody/constraint/constraint_solver.h" #include "drake/multibody/rigid_body_plant/compliant_contact_model.h" #include "drake/multibody/rigid_body_plant/kinematics_results.h" #include "drake/multibody/rigid_body_tree.h" @@ -395,8 +396,48 @@ class RigidBodyPlant : public LeafSystem { void ExportModelInstanceCentricPorts(); + void CalcContactStiffnessDampingMuAndNumHalfConeEdges( + const drake::multibody::collision::PointPair& contact, + double* stiffness, + double* damping, + double* mu, + int* num_cone_edges) const; + + Vector3 CalcRelTranslationalVelocity( + const KinematicsCache& kcache, int body_a_index, int body_b_index, + const Vector3& p_W) const; + + void UpdateGeneralizedForce( + const KinematicsCache& kcache, int body_a_index, int body_b_index, + const Vector3& p, const Vector3& f, VectorX* gf) const; + + VectorX ContactNormalJacobianMult( + const std::vector& contacts, + const VectorX& q, + const VectorX& v) const; + + VectorX TransposedContactNormalJacobianMult( + const std::vector& contacts, + const KinematicsCache& kcache, + const VectorX& f) const; + + VectorX ContactTangentJacobianMult( + const std::vector& contacts, + const VectorX& q, + const VectorX& v, + const std::vector& half_num_cone_edges) const; + + VectorX TransposedContactTangentJacobianMult( + const std::vector& contacts, + const KinematicsCache& kcache, + const VectorX& f, + const std::vector& half_num_cone_edges) const; + std::unique_ptr> tree_; + // Object that performs all constraint computations. + multibody::constraint::ConstraintSolver constraint_solver_; + OutputPortIndex state_output_port_index_{}; OutputPortIndex kinematics_output_port_index_{}; OutputPortIndex contact_output_port_index_{}; @@ -429,6 +470,19 @@ class RigidBodyPlant : public LeafSystem { // Pointer to the class that encapsulates all the contact computations. const std::unique_ptr> compliant_contact_model_; + + // Structure for storing joint limit data for time stepping. + struct JointLimit { + // The index for the joint limit. + int v_index{-1}; + + // Whether the limit is a lower limit or upper limit. + bool lower_limit{false}; + + // The signed distance from the limit. Negative signed distances correspond + // to joint limit violations. + T signed_distance{0}; + }; }; } // namespace systems