diff --git a/config/simulator.yaml b/config/simulator.yaml index 46934c08..45a259b7 100644 --- a/config/simulator.yaml +++ b/config/simulator.yaml @@ -9,6 +9,7 @@ simulator: save_rate: 1.0 save_history: 4096 headless: false + motor_timeout: 100 ref_heading: 90.0 # For the GPS sensor to work diff --git a/simulator/simulator.cpp b/simulator/simulator.cpp index e8f19e9c..897a1c99 100644 --- a/simulator/simulator.cpp +++ b/simulator/simulator.cpp @@ -18,6 +18,8 @@ namespace mrover { mIkTargetPub = create_publisher("arm_ik", 1); + mMotorTimeoutMs = get_parameter("motor_timeout").as_int(); + mIsHeadless = get_parameter("headless").as_bool(); mEnablePhysics = mIsHeadless; { diff --git a/simulator/simulator.hpp b/simulator/simulator.hpp index ae71c623..a6fde183 100644 --- a/simulator/simulator.hpp +++ b/simulator/simulator.hpp @@ -84,6 +84,7 @@ namespace mrover { int index{}; boost::container::small_vector, 2> visualUniforms; boost::container::small_vector, 2> collisionUniforms; + Clock::time_point lastUpdate = Clock::now(); }; urdf::Model model; @@ -263,6 +264,8 @@ namespace mrover { bool mIsHeadless{}; + int64_t mMotorTimeoutMs{}; + // Rendering GlfwInstance mGlfwInstance; @@ -362,7 +365,7 @@ namespace mrover { } if (auto it = mUrdfs.find("rover"); it != mUrdfs.end()) { - URDF const& rover = it->second; + URDF& rover = it->second; for (std::size_t i = 0; i < names.size(); ++i) { std::string const& name = names[i]; @@ -375,7 +378,8 @@ namespace mrover { } std::string const& urdfName = it->second; - URDF::LinkMeta const& linkMeta = rover.linkNameToMeta.at(urdfName); + URDF::LinkMeta& linkMeta = rover.linkNameToMeta.at(urdfName); + linkMeta.lastUpdate = Clock::now(); auto* motor = std::bit_cast(rover.physics->getLink(linkMeta.index).m_userPtr); assert(motor); diff --git a/simulator/simulator.physics.cpp b/simulator/simulator.physics.cpp index ff73de08..7e5c33de 100644 --- a/simulator/simulator.physics.cpp +++ b/simulator/simulator.physics.cpp @@ -53,6 +53,19 @@ namespace mrover { motor->setMaxAppliedImpulse(0.5); motor->setPositionTarget(0); } + // check if arm motor commands have expired + // TODO: fix hard-coded names? + for (auto const& name: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { + bool expired = std::chrono::duration_cast(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > mMotorTimeoutMs; + if (expired) { + int linkIndex = rover.linkNameToMeta.at(name).index; + auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr); + assert(motor); + motor->setVelocityTarget(0, 1); + // set p gain to 0 to stop position control + motor->setPositionTarget(0, 0); + } + } } float updateDuration = std::clamp(std::chrono::duration_cast>(dt).count(), 0.0f, 0.1f);