Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Sim motor timeouts #30

Merged
merged 5 commits into from
Dec 3, 2024
Merged

Sim motor timeouts #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f8db485
added arm motor timeouts to sim
Crypt1cG Nov 2, 2024
aff4828
motor timeouts
Crypt1cG Nov 4, 2024
899b524
minor tweaks
Crypt1cG Nov 4, 2024
d3c4b2d
changed motor timeout to ros param
Crypt1cG Nov 24, 2024
eaba812
fix style check
Crypt1cG Nov 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions config/simulator.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 2 additions & 0 deletions simulator/simulator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@ namespace mrover {

mIkTargetPub = create_publisher<msg::IK>("arm_ik", 1);

mMotorTimeoutMs = get_parameter("motor_timeout").as_int();

mIsHeadless = get_parameter("headless").as_bool();
mEnablePhysics = mIsHeadless;
{
Expand Down
8 changes: 6 additions & 2 deletions simulator/simulator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,7 @@ namespace mrover {
int index{};
boost::container::small_vector<Uniform<ModelUniforms>, 2> visualUniforms;
boost::container::small_vector<Uniform<ModelUniforms>, 2> collisionUniforms;
Clock::time_point lastUpdate = Clock::now();
};

urdf::Model model;
Expand Down Expand Up @@ -263,6 +264,8 @@ namespace mrover {

bool mIsHeadless{};

int64_t mMotorTimeoutMs{};

// Rendering

GlfwInstance mGlfwInstance;
Expand Down Expand Up @@ -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];
Expand All @@ -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<btMultiBodyJointMotor*>(rover.physics->getLink(linkMeta.index).m_userPtr);
assert(motor);
Expand Down
13 changes: 13 additions & 0 deletions simulator/simulator.physics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::chrono::milliseconds>(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > mMotorTimeoutMs;
if (expired) {
int linkIndex = rover.linkNameToMeta.at(name).index;
auto* motor = std::bit_cast<btMultiBodyJointMotor*>(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<std::chrono::duration<float>>(dt).count(), 0.0f, 0.1f);
Expand Down