Skip to content

Commit

Permalink
[core/python] Expose more convenience getters.
Browse files Browse the repository at this point in the history
  • Loading branch information
@duburcqa
duburcqa committed Mar 22, 2024
1 parent 0c41abe commit a308118
Show file tree
Hide file tree
Showing 5 changed files with 28 additions and 20 deletions.
2 changes: 1 addition & 1 deletion core/include/jiminy/core/engine/engine.h
View file
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
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
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
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
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

0 comments on commit a308118

Please sign in to comment.