search

RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.27k stars 1.26k forks source link

MBP segfaults on computing reaction forces when all joints are welds #15627

Closed edrumwri closed 3 years ago

edrumwri commented 3 years ago

Following code triggers it:

#include <memory>

#include "drake/common/drake_assert.h"
#include "drake/multibody/plant/multibody_plant.h"

#include <gtest/gtest.h>

using drake::geometry::Box;
using drake::math::RigidTransform;
using drake::multibody::Body;
using drake::multibody::MultibodyPlant;
using drake::multibody::SpatialForce;
using drake::multibody::SpatialInertia;
using drake::multibody::UnitInertia;
using drake::multibody::WeldJoint;
using drake::systems::Context;
using drake::Vector3;

class WeldedBoxesTest : public ::testing::Test {
 protected:
  void SetUp() final {
    plant_ = std::make_unique<MultibodyPlant<double>>(0.0 /* arbitrary continuous time */);
    AddBoxes();
    plant_->Finalize();
    plant_context_ = plant_->CreateDefaultContext();
  }

  void AddBoxes() {
    const double kCubeSize = 1.0;
    const double kBoxMass = 2.0;
    const Vector3<double> p_BoBcm_B = Vector3<double>::Zero();
    const UnitInertia<double> G_BBcm = UnitInertia<double>::SolidBox(kCubeSize, kCubeSize, kCubeSize);
    const SpatialInertia<double> M_BBo_B = SpatialInertia<double>::MakeFromCentralInertia(kBoxMass, p_BoBcm_B, G_BBcm);

    // Create two rigid bodies.
    boxA_ = &plant_->AddRigidBody("boxA", M_BBo_B);
    boxB_ = &plant_->AddRigidBody("boxB", M_BBo_B);

    // Desired transformation for the boxes in the world.
    const RigidTransform<double> X_WA(Vector3<double>::Zero());
    const RigidTransform<double> X_WB(Vector3<double>(kCubeSize, 0, 0));
    const RigidTransform<double> X_AB = X_WA.inverse() * X_WB;

    // Pin boxA to the world and boxB to boxA with weld joints.
    weld1_ = &plant_->WeldFrames(plant_->world_body().body_frame(), boxA_->body_frame(), X_WA);
    weld2_ = &plant_->WeldFrames(boxA_->body_frame(), boxB_->body_frame(), X_AB);
  }

  void VerifyBodyReactionForces() {
    const auto& reaction_forces_F_CJc_Jc =
        plant_->get_reaction_forces_output_port().Eval<std::vector<SpatialForce<double>>>(*plant_context_);
    (void)reaction_forces_F_CJc_Jc;
  }

  const Body<double>* boxA_{nullptr};
  const Body<double>* boxB_{nullptr};
  std::unique_ptr<MultibodyPlant<double>> plant_{nullptr};
  const WeldJoint<double>* weld1_{nullptr};
  const WeldJoint<double>* weld2_{nullptr};
  std::unique_ptr<Context<double>> plant_context_;
};

TEST_F(WeldedBoxesTest, Go) { VerifyBodyReactionForces(); }

Backtrace:

#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff795a859 in __GI_abort () at abort.c:79
#2  0x00005555566c0ed1 in drake::internal::Abort (condition=0x5555569dfff8 "start_index_in_v < this->get_parent_tree().num_velocities()", func=0x5555569dff90 "GetJacobianFromArray", 
    file=0x5555569dfed8 "bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/body_node.h", line=870) at common/drake_assert_and_throw.cc:49
#3  0x00005555566c10d2 in drake::internal::AssertionFailed (condition=0x5555569dfff8 "start_index_in_v < this->get_parent_tree().num_velocities()", func=0x5555569dff90 "GetJacobianFromArray", 
    file=0x5555569dfed8 "bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/body_node.h", line=870) at common/drake_assert_and_throw.cc:66
#4  0x0000555555f87004 in drake::multibody::internal::BodyNode<double>::GetJacobianFromArray (this=0x5555574fbad0, H_array=std::vector of length 0, capacity 0)
    at bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/body_node.h:870
#5  0x0000555555f4d1c6 in drake::multibody::internal::MultibodyTree<double>::CalcArticulatedBodyInertiaCache (this=0x5555574f9600, context=..., abic=0x5555575076f0) at multibody/tree/multibody_tree.cc:2818
#6  0x0000555556115518 in drake::multibody::internal::MultibodyTreeSystem<double>::CalcArticulatedBodyInertiaCache (this=0x5555574f1540, context=..., abi_cache=0x5555575076f0)
    at bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/multibody_tree_system.h:407
#7  0x000055555612bd8d in drake::systems::ValueProducer::make_calc_mode_1<drake::systems::SystemBase, drake::multibody::internal::MultibodyTreeSystem<double>, drake::systems::Context<double>, drake::multibody::internal::ArticulatedBodyInertiaCache<double> >(drake::systems::SystemBase const*, void (drake::multibody::internal::MultibodyTreeSystem<double>::*)(drake::systems::Context<double> const&, drake::multibody::internal::ArticulatedBodyInertiaCache<double>*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (
    this=0x5555574febb0, context=..., result=0x5555575076e0) at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/value_producer/drake/systems/framework/value_producer.h:450
#8  0x0000555556138329 in std::_Function_handler<void (drake::systems::ContextBase const&, drake::AbstractValue*), drake::systems::ValueProducer::make_calc_mode_1<drake::systems::SystemBase, drake::multibody::internal::MultibodyTreeSystem<double>, drake::systems::Context<double>, drake::multibody::internal::ArticulatedBodyInertiaCache<double> >(drake::systems::SystemBase const*, void (drake::multibody::internal::MultibodyTreeSystem<double>::*)(drake::systems::Context<double> const&, drake::multibody::internal::ArticulatedBodyInertiaCache<double>*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}>::_M_invoke(std::_Any_data const&, drake::systems::ContextBase const&, drake::AbstractValue*&&) (__functor=..., __args#0=..., __args#1=@0x7fffffffcda8: 0x5555575076e0)
    at /usr/include/c++/9/bits/std_function.h:300
#9  0x000055555659833b in std::function<void (drake::systems::ContextBase const&, drake::AbstractValue*)>::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (this=0x5555574ffc60, 
    __args#0=..., __args#1=0x5555575076e0) at /usr/include/c++/9/bits/std_function.h:688
#10 0x0000555556597f94 in drake::systems::ValueProducer::Calc (this=0x5555574ffc40, context=..., output=0x5555575076e0) at systems/framework/value_producer.cc:53
#11 0x000055555659620e in drake::systems::CacheEntry::Calc (this=0x5555574ffc10, context=..., value=0x5555575076e0) at systems/framework/cache_entry.cc:64
#12 0x000055555563626a in drake::systems::CacheEntry::UpdateValue (this=0x5555574ffc10, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:329
#13 0x00005555556360fe in drake::systems::CacheEntry::EvalAbstract (this=0x5555574ffc10, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:175
#14 0x000055555611e9ef in drake::systems::CacheEntry::Eval<drake::multibody::internal::ArticulatedBodyInertiaCache<double> > (this=0x5555574ffc10, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:159
#15 0x0000555556111fd3 in drake::multibody::internal::MultibodyTreeSystem<double>::EvalArticulatedBodyInertiaCache (this=0x5555574f1540, context=...)
    at bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/multibody_tree_system.h:131
#16 0x0000555555f4f36e in drake::multibody::internal::MultibodyTree<double>::EvalArticulatedBodyInertiaCache (this=0x5555574f9600, context=...)
    at bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/multibody_tree.h:2196
#17 0x0000555555f4d3b1 in drake::multibody::internal::MultibodyTree<double>::CalcArticulatedBodyForceCache (this=0x5555574f9600, context=..., forces=..., aba_force_cache=0x5555575083a0)
    at multibody/tree/multibody_tree.cc:2841
#18 0x0000555556115744 in drake::multibody::internal::MultibodyTreeSystem<double>::CalcArticulatedBodyForceCache (this=0x5555574f1540, context=..., aba_force_cache=0x5555575083a0)
    at multibody/tree/multibody_tree_system.cc:376
#19 0x000055555612c0c9 in drake::systems::ValueProducer::make_calc_mode_1<drake::systems::SystemBase, drake::multibody::internal::MultibodyTreeSystem<double>, drake::systems::Context<double>, drake::multibody::internal::ArticulatedBodyForceCache<double> >(drake::systems::SystemBase const*, void (drake::multibody::internal::MultibodyTreeSystem<double>::*)(drake::systems::Context<double> const&, drake::multibody::internal::ArticulatedBodyForceCache<double>*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (
    this=0x5555574fe7a0, context=..., result=0x555557508390) at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/value_producer/drake/systems/framework/value_producer.h:450
#20 0x00005555561386fd in std::_Function_handler<void (drake::systems::ContextBase const&, drake::AbstractValue*), drake::systems::ValueProducer::make_calc_mode_1<drake::systems::SystemBase, drake::multibody::internal::MultibodyTreeSystem<double>, drake::systems::Context<double>, drake::multibody::internal::ArticulatedBodyForceCache<double> >(drake::systems::SystemBase const*, void (drake::multibody::internal::MultibodyTreeSystem<double>::*)(drake::systems::Context<double> const&, drake::multibody::internal::ArticulatedBodyForceCache<double>*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}>::_M_invoke(std::_Any_data const&, drake::systems::ContextBase const&, drake::AbstractValue*&&) (__functor=..., __args#0=..., __args#1=@0x7fffffffd2b8: 0x555557508390) at /usr/include/c++/9/bits/std_function.h:300
#21 0x000055555659833b in std::function<void (drake::systems::ContextBase const&, drake::AbstractValue*)>::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (this=0x5555574ffe80, 
    __args#0=..., __args#1=0x555557508390) at /usr/include/c++/9/bits/std_function.h:688
#22 0x0000555556597f94 in drake::systems::ValueProducer::Calc (this=0x5555574ffe60, context=..., output=0x555557508390) at systems/framework/value_producer.cc:53
#23 0x000055555659620e in drake::systems::CacheEntry::Calc (this=0x5555574ffe30, context=..., value=0x555557508390) at systems/framework/cache_entry.cc:64
#24 0x000055555563626a in drake::systems::CacheEntry::UpdateValue (this=0x5555574ffe30, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:329
#25 0x00005555556360fe in drake::systems::CacheEntry::EvalAbstract (this=0x5555574ffe30, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:175
#26 0x000055555611eac7 in drake::systems::CacheEntry::Eval<drake::multibody::internal::ArticulatedBodyForceCache<double> > (this=0x5555574ffe30, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:159
#27 0x0000555556112257 in drake::multibody::internal::MultibodyTreeSystem<double>::EvalArticulatedBodyForceCache (this=0x5555574f1540, context=...)
    at bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/multibody_tree_system.h:211
#28 0x0000555556115841 in drake::multibody::internal::MultibodyTreeSystem<double>::CalcForwardDynamicsContinuous (this=0x5555574f1540, context=..., ac=0x5555575085b0)
    at multibody/tree/multibody_tree_system.cc:387
#29 0x00005555561157d0 in drake::multibody::internal::MultibodyTreeSystem<double>::CalcForwardDynamics (this=0x5555574f1540, context=..., ac=0x5555575085b0)
    at bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/multibody_tree_system.h:457
#30 0x000055555612c24d in drake::systems::ValueProducer::make_calc_mode_1<drake::systems::SystemBase, drake::multibody::internal::MultibodyTreeSystem<double>, drake::systems::Context<double>, drake::multibody::internal::AccelerationKinematicsCache<double> >(drake::systems::SystemBase const*, void (drake::multibody::internal::MultibodyTreeSystem<double>::*)(drake::systems::Context<double> const&, drake::multibody::internal::AccelerationKinematicsCache<double>*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (
    this=0x5555574fff10, context=..., result=0x5555575085a0) at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/value_producer/drake/systems/framework/value_producer.h:450
#31 0x000055555613891d in std::_Function_handler<void (drake::systems::ContextBase const&, drake::AbstractValue*), drake::systems::ValueProducer::make_calc_mode_1<drake::systems::SystemBase, drake::multibody::internal::MultibodyTreeSystem<double>, drake::systems::Context<double>, drake::multibody::internal::AccelerationKinematicsCache<double> >(drake::systems::SystemBase const*, void (drake::multibody::internal::MultibodyTreeSystem<double>::*)(drake::systems::Context<double> const&, drake::multibody::internal::AccelerationKinematicsCache<double>*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}>::_M_invoke(std::_Any_data const&, drake::systems::ContextBase const&, drake::AbstractValue*&&) (__functor=..., __args#0=..., __args#1=@0x7fffffffd528: 0x5555575085a0)
    at /usr/include/c++/9/bits/std_function.h:300
#32 0x000055555659833b in std::function<void (drake::systems::ContextBase const&, drake::AbstractValue*)>::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (this=0x555557500080, 
    __args#0=..., __args#1=0x5555575085a0) at /usr/include/c++/9/bits/std_function.h:688
#33 0x0000555556597f94 in drake::systems::ValueProducer::Calc (this=0x555557500060, context=..., output=0x5555575085a0) at systems/framework/value_producer.cc:53
#34 0x000055555659620e in drake::systems::CacheEntry::Calc (this=0x555557500030, context=..., value=0x5555575085a0) at systems/framework/cache_entry.cc:64
#35 0x000055555563626a in drake::systems::CacheEntry::UpdateValue (this=0x555557500030, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:329
#36 0x00005555556360fe in drake::systems::CacheEntry::EvalAbstract (this=0x555557500030, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:175
#37 0x000055555611e9a7 in drake::systems::CacheEntry::Eval<drake::multibody::internal::AccelerationKinematicsCache<double> > (this=0x555557500030, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:159
#38 0x0000555556111f77 in drake::multibody::internal::MultibodyTreeSystem<double>::EvalForwardDynamics (this=0x5555574f1540, context=...)
    at bazel-out/k8-dbg/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/multibody_tree_system.h:120
#39 0x000055555564aa7c in drake::multibody::MultibodyPlant<double>::CalcReactionForces (this=0x5555574f1540, context=..., F_CJc_Jc_array=0x55555750b480) at multibody/plant/multibody_plant.cc:3215
#40 0x00005555556f33e1 in drake::systems::ValueProducer::make_calc_mode_1<drake::systems::LeafSystem<double>, drake::multibody::MultibodyPlant<double>, drake::systems::Context<double>, std::vector<drake::multibody::SpatialForce<double>, std::allocator<drake::multibody::SpatialForce<double> > > >(drake::systems::LeafSystem<double> const*, void (drake::multibody::MultibodyPlant<double>::*)(drake::systems::Context<double> const&, std::vector<drake::multibody::SpatialForce<double>, std::allocator<drake::multibody::SpatialForce<double> > >*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (this=0x555557502dc0, context=..., result=0x55555750b470)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/value_producer/drake/systems/framework/value_producer.h:450
#41 0x000055555573f50f in std::_Function_handler<void (drake::systems::ContextBase const&, drake::AbstractValue*), drake::systems::ValueProducer::make_calc_mode_1<drake::systems::LeafSystem<double>, drake::multibody::MultibodyPlant<double>, drake::systems::Context<double>, std::vector<drake::multibody::SpatialForce<double>, std::allocator<drake::multibody::SpatialForce<double> > > >(drake::systems::LeafSystem<double> const*, void (drake::multibody::MultibodyPlant<double>::*)(drake::systems::Context<double> const&, std::vector<drake::multibody::SpatialForce<double>, std::allocator<drake::multibody::SpatialForce<double> > >*) const)::{lambda(drake::systems::ContextBase const&, drake::AbstractValue*)#1}>::_M_invoke(std::_Any_data const&, drake::systems::ContextBase const&, drake::AbstractValue*&&) (__functor=..., __args#0=..., 
    __args#1=@0x7fffffffd9f8: 0x55555750b470) at /usr/include/c++/9/bits/std_function.h:300
#42 0x000055555659833b in std::function<void (drake::systems::ContextBase const&, drake::AbstractValue*)>::operator()(drake::systems::ContextBase const&, drake::AbstractValue*) const (this=0x555557503530, 
    __args#0=..., __args#1=0x55555750b470) at /usr/include/c++/9/bits/std_function.h:688
#43 0x0000555556597f94 in drake::systems::ValueProducer::Calc (this=0x555557503510, context=..., output=0x55555750b470) at systems/framework/value_producer.cc:53
#44 0x000055555659620e in drake::systems::CacheEntry::Calc (this=0x5555575034e0, context=..., value=0x55555750b470) at systems/framework/cache_entry.cc:64
#45 0x000055555563626a in drake::systems::CacheEntry::UpdateValue (this=0x5555575034e0, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:329
#46 0x00005555556360fe in drake::systems::CacheEntry::EvalAbstract (this=0x5555575034e0, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/cache_entry/drake/systems/framework/cache_entry.h:175
#47 0x0000555556511840 in drake::systems::LeafOutputPort<double>::DoEval (this=0x555557503590, context=...)
    at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/leaf_output_port/drake/systems/framework/leaf_output_port.h:105
#48 0x000055555560b1b8 in drake::systems::OutputPort<double>::Eval<std::vector<drake::multibody::SpatialForce<double>, std::allocator<drake::multibody::SpatialForce<double> > >, void> (this=0x555557503590, 
    context=...) at bazel-out/k8-dbg/bin/systems/framework/_virtual_includes/output_port/drake/systems/framework/output_port.h:119
#49 0x0000555555609cd7 in WeldedBoxesTest::VerifyBodyReactionForces (this=0x5555574ecd80) at multibody/plant/test/simulated_force_torque_measurement_test.cc:58
#50 0x00005555556063c6 in WeldedBoxesTest_Go_Test::TestBody (this=0x5555574ecd80) at multibody/plant/test/simulated_force_torque_measurement_test.cc:73
sherm1 commented 3 years ago

@amcastro-tri would you take a look at this please? It appears that the ArticulatedInertia calculation barfs when there is no articulation!

amcastro-tri commented 3 years ago

Thanks for reporting. This is an interesting use case, essentially you are trying to compute the reaction forces in the "static condition" imposed by these welds. Very nice. Thank you for providing the unit test, I will definitely use it in the fix PR.

edrumwri commented 3 years ago

Hey @amcastro-tri can you provide an ETA on fixing this? We're depending on this feature (computing reaction forces for static bodies) to test a calculation. I don't feel confident providing a fix for this.

amcastro-tri commented 3 years ago

I do not have an ETA since I don't really know what the underlying problem is. I might be able to say more today evening. For sure I will work on this on Thursday morning.

amcastro-tri commented 3 years ago

Sorry for the delay all, working on diagnosing this today.

amcastro-tri commented 3 years ago

Ok @edrumwri, at least there is an easy fix: Switch to discrete mode. In discrete mode your test works and the numerical values are correct.

Definitely there is a bug in the continuous case. I'll debug that next.

amcastro-tri commented 3 years ago

I was able to find the problem. I will push a fix shortly.

amcastro-tri commented 3 years ago

@edrumwri, fix in #15686.