Skip to content

Commit

Permalink
[gym_jiminy/common] Various minor improvements. (#849)
Browse files Browse the repository at this point in the history 
* [core] Properly handle undefined motor velocity bounds.
* [core] Rename 'positionLimit(Min|Max)' in 'positionLimit(Lower|Upper)' for consistency with Pinocchio.
* [gym_jiminy/common] More robust random seed initialization.
* [gym_jiminy/common] Add option to avoid clipping target motor accelerations of PD controller.
* [gym_jiminy/common] Do not overwrite 'BaseJiminyEnv.(observation|action)' if already defined.
* [gym_jiminy/common] Support additional 'special' string when loading pipeline from toml.
* [gym_jiminy/common] 'ConvexHull2D.get_distance_to_point' now supports single query.
* [gym_jiminy/common] Try initializing quantities even if no simulation is running.
* [gym_jiminy/common] Clear exception message if nested key is missing.
* [gym_jiminy/common] Move leaky twist from mahony filter to body observer.
* [gym_jiminy/common] Add target joint velocity deadband to PD controller.
* [gym_jiminy/common] Fix 'ControlledJiminyEnv.action' not reset during setup.
* [gym_jiminy/common] Increase simulation timeout delay.
  • Loading branch information
@duburcqa
duburcqa authored Dec 19, 2024
1 parent 0373511 commit a30b5e9
Show file tree
Hide file tree
Showing 21 changed files with 362 additions and 281 deletions.
12 changes: 6 additions & 6 deletions core/include/jiminy/core/robot/model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,8 +137,8 @@ namespace jiminy
{
GenericConfig config;
config["positionLimitFromUrdf"] = true;
config["positionLimitMin"] = Eigen::VectorXd{};
config["positionLimitMax"] = Eigen::VectorXd{};
config["positionLimitLower"] = Eigen::VectorXd{};
config["positionLimitUpper"] = Eigen::VectorXd{};

return config;
};
Expand Down Expand Up @@ -181,13 +181,13 @@ namespace jiminy
{
const bool positionLimitFromUrdf;
/// \brief Min position limit of all the mechanical joints of the theoretical model.
const Eigen::VectorXd positionLimitMin;
const Eigen::VectorXd positionLimitMax;
const Eigen::VectorXd positionLimitLower;
const Eigen::VectorXd positionLimitUpper;

JointOptions(const GenericConfig & options) :
positionLimitFromUrdf{boost::get<bool>(options.at("positionLimitFromUrdf"))},
positionLimitMin{boost::get<Eigen::VectorXd>(options.at("positionLimitMin"))},
positionLimitMax{boost::get<Eigen::VectorXd>(options.at("positionLimitMax"))}
positionLimitLower{boost::get<Eigen::VectorXd>(options.at("positionLimitLower"))},
positionLimitUpper{boost::get<Eigen::VectorXd>(options.at("positionLimitUpper"))}
{
}
};
Expand Down
26 changes: 13 additions & 13 deletions core/src/engine/engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3238,8 +3238,8 @@ namespace jiminy
{
typedef boost::fusion::vector<
const Eigen::VectorXd & /* q */,
const Eigen::VectorXd & /* positionLimitMin */,
const Eigen::VectorXd & /* positionLimitMax */,
const Eigen::VectorXd & /* positionLimitLower */,
const Eigen::VectorXd & /* positionLimitUpper */,
const std::unique_ptr<const Engine::EngineOptions> & /* engineOptions */,
const std::shared_ptr<AbstractConstraintBase> & /* constraint */>
ArgsType;
Expand All @@ -3252,16 +3252,16 @@ namespace jiminy
void>
algo(const pinocchio::JointModelBase<JointModel> & joint,
const Eigen::VectorXd & q,
const Eigen::VectorXd & positionLimitMin,
const Eigen::VectorXd & positionLimitMax,
const Eigen::VectorXd & positionLimitLower,
const Eigen::VectorXd & positionLimitUpper,
const std::unique_ptr<const Engine::EngineOptions> & engineOptions,
const std::shared_ptr<AbstractConstraintBase> & constraint)
{
// Define some proxies for convenience
const Eigen::Index positionIndex = joint.idx_q();
const double qJoint = q[positionIndex];
const double qJointMin = positionLimitMin[positionIndex];
const double qJointMax = positionLimitMax[positionIndex];
const double qJointMin = positionLimitLower[positionIndex];
const double qJointMax = positionLimitUpper[positionIndex];
const double transitionEps = engineOptions->contacts.transitionEps;

// Check if out-of-bounds
Expand All @@ -3287,8 +3287,8 @@ namespace jiminy
void>
algo(const pinocchio::JointModelBase<JointModel> & /* joint */,
const Eigen::VectorXd & /* q */,
const Eigen::VectorXd & /* positionLimitMin */,
const Eigen::VectorXd & /* positionLimitMax */,
const Eigen::VectorXd & /* positionLimitLower */,
const Eigen::VectorXd & /* positionLimitUpper */,
const std::unique_ptr<const Engine::EngineOptions> & /* engineOptions */,
const std::shared_ptr<AbstractConstraintBase> & constraint)
{
Expand All @@ -3307,8 +3307,8 @@ namespace jiminy
void>
algo(const pinocchio::JointModelBase<JointModel> & /* joint */,
const Eigen::VectorXd & /* q */,
const Eigen::VectorXd & /* positionLimitMin */,
const Eigen::VectorXd & /* positionLimitMax */,
const Eigen::VectorXd & /* positionLimitLower */,
const Eigen::VectorXd & /* positionLimitUpper */,
const std::unique_ptr<const Engine::EngineOptions> & /* engineOptions */,
const std::shared_ptr<AbstractConstraintBase> & constraint)
{
Expand All @@ -3332,8 +3332,8 @@ namespace jiminy

/* Enforce position limits for all joints having bounds constraints, ie mechanical and
backlash joints. */
const Eigen::VectorXd & positionLimitMin = robot->pinocchioModel_.lowerPositionLimit;
const Eigen::VectorXd & positionLimitMax = robot->pinocchioModel_.upperPositionLimit;
const Eigen::VectorXd & positionLimitLower = robot->pinocchioModel_.lowerPositionLimit;
const Eigen::VectorXd & positionLimitUpper = robot->pinocchioModel_.upperPositionLimit;
for (auto & constraintItem : constraints.boundJoints)
{
auto & constraint = constraintItem.second;
Expand All @@ -3342,7 +3342,7 @@ namespace jiminy
computePositionLimitsForcesAlgo::run(
model.joints[jointIndex],
typename computePositionLimitsForcesAlgo::ArgsType(
q, positionLimitMin, positionLimitMax, engineOptions_, constraint));
q, positionLimitLower, positionLimitUpper, engineOptions_, constraint));
}

// Compute the flexibilities (only support `JointModelType::SPHERICAL` so far)
Expand Down
19 changes: 13 additions & 6 deletions core/src/hardware/basic_motors.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,10 @@ namespace jiminy
{
// Check if the friction parameters make sense
// Make sure the user-defined position limit has the right dimension
if (boost::get<double>(motorOptions.at("velocityEffortInvSlope")) < 0.0)
{
JIMINY_THROW(std::invalid_argument, "'velocityEffortInvSlope' must be positive.");
}
if (boost::get<double>(motorOptions.at("frictionViscousPositive")) > 0.0)
{
JIMINY_THROW(std::invalid_argument, "'frictionViscousPositive' must be negative.");
Expand Down Expand Up @@ -104,12 +108,15 @@ namespace jiminy
effortMax = effortLimit_;
if (motorOptions_->enableVelocityLimit)
{
const double velocityThr = std::max(
velocityLimit_ - effortLimit_ * motorOptions_->velocityEffortInvSlope, 0.0);
effortMin *= std::clamp(
(velocityLimit_ + vMotor) / (velocityLimit_ - velocityThr), 0.0, 1.0);
effortMax *= std::clamp(
(velocityLimit_ - vMotor) / (velocityLimit_ - velocityThr), 0.0, 1.0);
const double velocityDelta = effortLimit_ * motorOptions_->velocityEffortInvSlope;
if (velocityDelta > 0.0)
{
const double velocityThr = std::max(velocityLimit_ - velocityDelta, 0.0);
effortMin *= std::clamp(
(velocityLimit_ + vMotor) / (velocityLimit_ - velocityThr), 0.0, 1.0);
effortMax *= std::clamp(
(velocityLimit_ - vMotor) / (velocityLimit_ - velocityThr), 0.0, 1.0);
}
}
}
uMotor = std::clamp(command, effortMin, effortMax);
Expand Down
54 changes: 27 additions & 27 deletions core/src/robot/model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1368,8 +1368,8 @@ namespace jiminy
}

// Re-initialize position limits
Eigen::VectorXd positionLimitMin = Eigen::VectorXd::Constant(pinocchioModel_.nq, -INF);
Eigen::VectorXd positionLimitMax = Eigen::VectorXd::Constant(pinocchioModel_.nq, +INF);
Eigen::VectorXd positionLimitLower = Eigen::VectorXd::Constant(pinocchioModel_.nq, -INF);
Eigen::VectorXd positionLimitUpper = Eigen::VectorXd::Constant(pinocchioModel_.nq, +INF);
Eigen::Index idx_q, nq;
for (const auto & joint : pinocchioModel_.joints)
{
Expand All @@ -1392,16 +1392,16 @@ namespace jiminy
default:
continue;
}
positionLimitMin.segment(idx_q, nq).setConstant(-1.0 - EPS);
positionLimitMax.segment(idx_q, nq).setConstant(+1.0 + EPS);
positionLimitLower.segment(idx_q, nq).setConstant(-1.0 - EPS);
positionLimitUpper.segment(idx_q, nq).setConstant(+1.0 + EPS);
}

// Copy backlash joint position limits as-is from extended model
for (pinocchio::JointIndex jointIndex : backlashJointIndices_)
{
const Eigen::Index positionIndex = pinocchioModel_.idx_qs[jointIndex];
positionLimitMin[positionIndex] = pinocchioModel_.lowerPositionLimit[positionIndex];
positionLimitMax[positionIndex] = pinocchioModel_.upperPositionLimit[positionIndex];
positionLimitLower[positionIndex] = pinocchioModel_.lowerPositionLimit[positionIndex];
positionLimitUpper[positionIndex] = pinocchioModel_.upperPositionLimit[positionIndex];
}

// Set mechanical joint position limits from theoretical model or user options
Expand All @@ -1418,9 +1418,9 @@ namespace jiminy
}
const auto & joint = pinocchioModel_.joints[jointIndex];
const auto & jointTh = pinocchioModelTh_.joints[jointIndexTh];
positionLimitMin.segment(joint.idx_q(), joint.nq()) =
positionLimitLower.segment(joint.idx_q(), joint.nq()) =
pinocchioModelTh_.lowerPositionLimit.segment(jointTh.idx_q(), jointTh.nq());
positionLimitMax.segment(joint.idx_q(), joint.nq()) =
positionLimitUpper.segment(joint.idx_q(), joint.nq()) =
pinocchioModelTh_.upperPositionLimit.segment(jointTh.idx_q(), jointTh.nq());
}
}
Expand All @@ -1429,14 +1429,14 @@ namespace jiminy
for (std::size_t i = 0; i < mechanicalJointPositionIndices_.size(); ++i)
{
Eigen::Index positionIndex = mechanicalJointPositionIndices_[i];
positionLimitMin[positionIndex] = modelOptions_->joints.positionLimitMin[i];
positionLimitMax[positionIndex] = modelOptions_->joints.positionLimitMax[i];
positionLimitLower[positionIndex] = modelOptions_->joints.positionLimitLower[i];
positionLimitUpper[positionIndex] = modelOptions_->joints.positionLimitUpper[i];
}
}

// Overwrite extended model position limits
pinocchioModel_.lowerPositionLimit = positionLimitMin;
pinocchioModel_.upperPositionLimit = positionLimitMax;
pinocchioModel_.lowerPositionLimit = positionLimitLower;
pinocchioModel_.upperPositionLimit = positionLimitUpper;

// Initialize backup joint space acceleration
jointSpatialAccelerations_ =
Expand Down Expand Up @@ -1557,33 +1557,33 @@ namespace jiminy
boost::get<bool>(jointOptionsHolder.at("positionLimitFromUrdf"));
if (!positionLimitFromUrdf)
{
const Eigen::VectorXd & jointsPositionLimitMin =
boost::get<Eigen::VectorXd>(jointOptionsHolder.at("positionLimitMin"));
const Eigen::VectorXd & jointspositionLimitLower =
boost::get<Eigen::VectorXd>(jointOptionsHolder.at("positionLimitLower"));
if (mechanicalJointPositionIndices_.size() !=
static_cast<uint32_t>(jointsPositionLimitMin.size()))
static_cast<uint32_t>(jointspositionLimitLower.size()))
{
JIMINY_THROW(std::invalid_argument,
"Wrong vector size for 'positionLimitMin'.");
"Wrong vector size for 'positionLimitLower'.");
}
const Eigen::VectorXd & jointsPositionLimitMax =
boost::get<Eigen::VectorXd>(jointOptionsHolder.at("positionLimitMax"));
const Eigen::VectorXd & jointspositionLimitUpper =
boost::get<Eigen::VectorXd>(jointOptionsHolder.at("positionLimitUpper"));
if (mechanicalJointPositionIndices_.size() !=
static_cast<uint32_t>(jointsPositionLimitMax.size()))
static_cast<uint32_t>(jointspositionLimitUpper.size()))
{
JIMINY_THROW(std::invalid_argument,
"Wrong vector size for 'positionLimitMax'.");
"Wrong vector size for 'positionLimitUpper'.");
}
if (mechanicalJointPositionIndices_.size() ==
static_cast<uint32_t>(modelOptions_->joints.positionLimitMin.size()))
static_cast<uint32_t>(modelOptions_->joints.positionLimitLower.size()))
{
auto jointsPositionLimitMinDiff =
jointsPositionLimitMin - modelOptions_->joints.positionLimitMin;
auto jointspositionLimitLowerDiff =
jointspositionLimitLower - modelOptions_->joints.positionLimitLower;
internalBuffersMustBeUpdated |=
(jointsPositionLimitMinDiff.array().abs() >= EPS).all();
auto jointsPositionLimitMaxDiff =
jointsPositionLimitMax - modelOptions_->joints.positionLimitMax;
(jointspositionLimitLowerDiff.array().abs() >= EPS).all();
auto jointspositionLimitUpperDiff =
jointspositionLimitUpper - modelOptions_->joints.positionLimitUpper;
internalBuffersMustBeUpdated |=
(jointsPositionLimitMaxDiff.array().abs() >= EPS).all();
(jointspositionLimitUpperDiff.array().abs() >= EPS).all();
}
else
{
Expand Down
4 changes: 2 additions & 2 deletions core/unit/engine_sanity_check.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,9 @@ TEST(EngineSanity, EnergyConservation)
GenericConfig & modelOptions = boost::get<GenericConfig>(robotOptions.at("model"));
GenericConfig & jointsOptions = boost::get<GenericConfig>(modelOptions.at("joints"));
boost::get<bool>(jointsOptions.at("positionLimitFromUrdf")) = false;
boost::get<Eigen::VectorXd>(jointsOptions.at("positionLimitMin")) =
boost::get<Eigen::VectorXd>(jointsOptions.at("positionLimitLower")) =
Eigen::Vector2d::Constant(-INF);
boost::get<Eigen::VectorXd>(jointsOptions.at("positionLimitMax")) =
boost::get<Eigen::VectorXd>(jointsOptions.at("positionLimitUpper")) =
Eigen::Vector2d::Constant(+INF);

// Disable velocity and torque limits
Expand Down
21 changes: 17 additions & 4 deletions python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
from .blocks import BaseControllerBlock, BaseObserverBlock

from ..utils import (DataNested,
fill,
zeros,
build_copyto,
copy,
Expand Down Expand Up @@ -368,13 +369,22 @@ def reset_hook() -> Optional[InterfaceJiminyEnv]:

return env_derived

# Reset base pipeline.
# Note that it is important to reset the base environment first,
# otherwise adding extra layers on top of an existing pipeline would
# affect random number sampling, and therefore mess up with
# repeatability.
obs, info = self.env.reset(
seed=seed, options={"reset_hook": reset_hook})

# Reset the seed of the action and observation spaces
if seed is not None:
self.observation_space.seed(seed)
self.action_space.seed(seed)
obs_seed, act_seed = map(int, self.np_random.integers(
np.iinfo(int).max, size=(2,), dtype=int))
self.observation_space.seed(obs_seed)
self.action_space.seed(act_seed)

# Reset base pipeline
return self.env.reset(seed=seed, options={"reset_hook": reset_hook})
return obs, info

def step(self, # type: ignore[override]
action: Act
Expand Down Expand Up @@ -1108,6 +1118,9 @@ def _setup(self) -> None:
# Call base implementation
super()._setup()

# Reset action
fill(self.action, 0)

# Compute the observe and control update periods
self.observe_dt = self.env.observe_dt
self.control_dt = self.controller.control_dt
Expand Down
9 changes: 7 additions & 2 deletions python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -782,8 +782,13 @@ def initialize(self) -> None:
# Call base implementation
super().initialize()

# Force initializing state quantity
self.state.initialize()
# Force initializing state quantity if possible
try:
self.state.initialize()
except RuntimeError:
# No simulation running. This may be problematic but it is not
# blocking at this point.
pass

# Refresh robot proxy
assert isinstance(self.state, StateQuantity)
Expand Down
Loading

0 comments on commit a30b5e9

Please sign in to comment.