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

[core/python] Expose more convenience getters. #748

Merged
merged 1 commit into from
Mar 23, 2024
Merged

[core/python] Expose more convenience getters. #748

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
2 changes: 1 addition & 1 deletion core/include/jiminy/core/engine/engine.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -664,7 +664,7 @@ namespace jiminy
GenericConfig getOptions() const noexcept;
void setOptions(const GenericConfig & engineOptions);
bool getIsTelemetryConfigured() const;
std::shared_ptr<Robot> & getRobot(const std::string & robotName);
std::shared_ptr<Robot> getRobot(const std::string & robotName);
std::ptrdiff_t getRobotIndex(const std::string & robotName) const;
const RobotState & getRobotState(const std::string & robotName) const;
const StepperState & getStepperState() const;
Expand Down
9 changes: 5 additions & 4 deletions core/src/engine/engine.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2317,11 +2317,12 @@ namespace jiminy
}
catch (const std::exception & e)
{
// TODO: Try using `std::throw_with_nested` instead
std::runtime_error(toString(
// TODO: Support `std::throw_with_nested` in THROW_ERROR instead
THROW_ERROR(
std::runtime_error,
"Something is wrong with the physics. Try using an adaptive stepper. "
"Aborting integration.\nRaised from exception: ",
e.what()));
e.what());
}
}
THROW_ERROR(std::runtime_error,
Expand Down Expand Up @@ -2776,7 +2777,7 @@ namespace jiminy
return std::distance(robots_.begin(), robotIt);
}

std::shared_ptr<Robot> & Engine::getRobot(const std::string & robotName)
std::shared_ptr<Robot> Engine::getRobot(const std::string & robotName)
{
std::ptrdiff_t robotIndex = getRobotIndex(robotName);
return robots_[robotIndex];
Expand Down
1 change: 0 additions & 1 deletion core/src/stepper/abstract_stepper.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,6 @@ namespace jiminy
}
catch (...)
{

return {stepper::ReturnCode::IS_ERROR, std::current_exception()};
}

Expand Down
30 changes: 16 additions & 14 deletions core/src/utilities/random.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -649,10 +649,12 @@ namespace jiminy
double orientation,
uint32_t seed)
{
if ((0.01 <= interpDelta.array()).all() &&
(interpDelta.array() <= size.array() / 2.0).all())
if ((0.01 < interpDelta.array()).any() || (interpDelta.array() > size.array() / 2.0).any())
{
PRINT_WARNING("'interpDelta' must be in range [0.01, 'size'/2.0].");
PRINT_WARNING(
"All components of 'interpDelta' must be in range [0.01, 'size'/2.0]. Value: ",
interpDelta.transpose(),
"'.");
}

Eigen::Vector2d interpThr = interpDelta.cwiseMax(0.01).cwiseMin(size / 2.0);
Expand Down Expand Up @@ -682,45 +684,45 @@ namespace jiminy
double dheight_x;
std::tie(height, dheight_x) = tile2dInterp1d(
posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed);
double const norm_inv = 1.0 / std::sqrt(dheight_x * dheight_x + 1.0);
const double norm_inv = 1.0 / std::sqrt(dheight_x * dheight_x + 1.0);
normal << -dheight_x * norm_inv, 0.0, norm_inv;
}
else if (!is_edge[0] && is_edge[1])
{
double dheight_y;
std::tie(height, dheight_y) = tile2dInterp1d(
posIndices, posRel, 1, size, sparsity, heightMax, interpThr, seed);
double const norm_inv = 1.0 / std::sqrt(dheight_y * dheight_y + 1.0);
const double norm_inv = 1.0 / std::sqrt(dheight_y * dheight_y + 1.0);
normal << 0.0, -dheight_y * norm_inv, norm_inv;
}
else if (is_edge[0] && is_edge[1])
{
auto const [height_0, dheight_x_0] = tile2dInterp1d(
const auto [height_0, dheight_x_0] = tile2dInterp1d(
posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed);
if (posRel[1] < interpThr[1])
{
posIndices[1] -= 1;
auto const [height_m, dheight_x_m] = tile2dInterp1d(
const auto [height_m, dheight_x_m] = tile2dInterp1d(
posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed);

double const ratio = (1.0 - posRel[1] / interpThr[1]) / 2.0;
const double ratio = (1.0 - posRel[1] / interpThr[1]) / 2.0;
height = height_0 + (height_m - height_0) * ratio;
double const dheight_x = dheight_x_0 + (dheight_x_m - dheight_x_0) * ratio;
double const dheight_y =
const double dheight_x = dheight_x_0 + (dheight_x_m - dheight_x_0) * ratio;
const double dheight_y =
(height_0 - height_m) / (2.0 * size[1] * interpThr[1]);
normal << -dheight_x, -dheight_y, 1.0;
normal.normalize();
}
else
{
posIndices[1] += 1;
auto const [height_p, dheight_x_p] = tile2dInterp1d(
const auto [height_p, dheight_x_p] = tile2dInterp1d(
posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed);

double const ratio = (1.0 + (posRel[1] - 1.0) / interpThr[1]) / 2.0;
const double ratio = (1.0 + (posRel[1] - 1.0) / interpThr[1]) / 2.0;
height = height_0 + (height_p - height_0) * ratio;
double const dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio;
double const dheight_y =
const double dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio;
const double dheight_y =
(height_p - height_0) / (2.0 * size[1] * interpThr[1]);
normal << -dheight_x, -dheight_y, 1.0;
normal.normalize();
Expand Down
6 changes: 6 additions & 0 deletions python/jiminy_pywrap/src/engine.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -771,6 +771,12 @@ namespace jiminy::python

.DEF_READONLY_WITH_POLICY(
"robots", &Engine::robots_, bp::return_value_policy<result_converter<true>>())
.def("get_robot", &Engine::getRobot, (bp::arg("self"), "robot_name"))
.def("get_robot_index", &Engine::getRobotIndex, (bp::arg("self"), "robot_name"))
.def("get_robot_state",
&Engine::getRobotState,
(bp::arg("self"), "robot_name"),
bp::return_value_policy<result_converter<false>>())

.ADD_PROPERTY_GET("robot_states", &internal::engine::getRobotStates)
.ADD_PROPERTY_GET_WITH_POLICY("stepper_state",
Expand Down
Loading