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

[Gym] Generic locomotion envs - PART VII #201

Merged
merged 9 commits into from
Sep 22, 2020
Merged

[Gym] Generic locomotion envs - PART VII #201

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
3334232
[misc] Remove Python 2 compatibility code.
Sep 22, 2020
98c8002
[core] Fix Model 'removeCollisionBodies' method.
Sep 22, 2020
b7c4cdc
[core] Fix support of multiple geometry by body for collision computa…
Sep 22, 2020
fd95bc4
[core] Compute the centers of mass systematically.
Sep 22, 2020
3de3e55
[core] Check that the initial state is not out of bounds.
Sep 22, 2020
b9a6c9d
[core|gym] Properly handle step integration failure.
Sep 22, 2020
cd57fc1
[gym] Fix 'done' flag handling in 'step' method. Remove 'engine' prop…
Sep 22, 2020
3c08752
[gym] Use sample_state to get initial height.
Sep 22, 2020
00564d1
[gym] Fix PD Control env motor-to-encoder mapping.
Sep 22, 2020
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/Utilities.tpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -189,7 +189,7 @@ namespace jiminy
void eraseVector(std::vector<T> & vect1,
std::vector<T> const & vect2)
{
vect1.erase(std::remove_if (vect1.begin(), vect1.end(),
vect1.erase(std::remove_if(vect1.begin(), vect1.end(),
[&vect2](auto const & elem1)
{
auto vect2It = std::find(vect2.begin(), vect2.end(), elem1);
Expand Down
42 changes: 22 additions & 20 deletions core/include/jiminy/core/robot/Model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,11 +153,11 @@ namespace jiminy
pinocchio::SE3 const & framePlacement);
hresult_t removeFrame(std::string const & frameName);
hresult_t addCollisionBodies(std::vector<std::string> const & bodyNames);
hresult_t removeCollisionBodies(std::vector<std::string> const & frameNames = {});
hresult_t removeCollisionBodies(std::vector<std::string> frameNames = {}); // Make a copy
hresult_t addContactPoints(std::vector<std::string> const & frameNames);
hresult_t removeContactPoints(std::vector<std::string> const & frameNames = {});

hresult_t setOptions(configHolder_t modelOptions); // Make a copy !
hresult_t setOptions(configHolder_t modelOptions); // Make a copy
configHolder_t getOptions(void) const;

/// This method are not intended to be called manually. The Engine is taking care of it.
Expand All @@ -175,6 +175,7 @@ namespace jiminy
std::vector<std::string> const & getCollisionBodiesNames(void) const;
std::vector<std::string> const & getContactFramesNames(void) const;
std::vector<int32_t> const & getCollisionBodiesIdx(void) const;
std::vector<std::vector<int32_t> > const & getCollisionPairsIdx(void) const;
std::vector<int32_t> const & getContactFramesIdx(void) const;
std::vector<std::string> const & getRigidJointsNames(void) const;
std::vector<int32_t> const & getRigidJointsModelIdx(void) const;
Expand Down Expand Up @@ -223,24 +224,25 @@ namespace jiminy
bool_t hasFreeflyer_;
configHolder_t mdlOptionsHolder_;

std::vector<std::string> collisionBodiesNames_; ///< Name of the collision bodies of the robot
std::vector<std::string> contactFramesNames_; ///< Name of the contact frames of the robot
std::vector<int32_t> collisionBodiesIdx_; ///< Indices of the collision bodies in the frame list of the robot
std::vector<int32_t> contactFramesIdx_; ///< Indices of the contact frames in the frame list of the robot
std::vector<std::string> rigidJointsNames_; ///< Name of the actual joints of the robot, not taking into account the freeflyer
std::vector<int32_t> rigidJointsModelIdx_; ///< Index of the actual joints in the pinocchio robot
std::vector<int32_t> rigidJointsPositionIdx_; ///< All the indices of the actual joints in the configuration vector of the robot (ie including all the degrees of freedom)
std::vector<int32_t> rigidJointsVelocityIdx_; ///< All the indices of the actual joints in the velocity vector of the robot (ie including all the degrees of freedom)
std::vector<std::string> flexibleJointsNames_; ///< Name of the flexibility joints of the robot regardless of whether the flexibilities are enable
std::vector<int32_t> flexibleJointsModelIdx_; ///< Index of the flexibility joints in the pinocchio robot regardless of whether the flexibilities are enable

vectorN_t positionLimitMin_; ///< Upper position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)

std::vector<std::string> positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the rigid robot
std::vector<std::string> velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the rigid robot
std::vector<std::string> accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the rigid robot
std::vector<std::string> collisionBodiesNames_; ///< Name of the collision bodies of the robot
std::vector<std::string> contactFramesNames_; ///< Name of the contact frames of the robot
std::vector<int32_t> collisionBodiesIdx_; ///< Indices of the collision bodies in the frame list of the robot
std::vector<std::vector<int32_t> > collisionPairsIdx_; ///< Indices of the collision pairs associated with each collision body
std::vector<int32_t> contactFramesIdx_; ///< Indices of the contact frames in the frame list of the robot
std::vector<std::string> rigidJointsNames_; ///< Name of the actual joints of the robot, not taking into account the freeflyer
std::vector<int32_t> rigidJointsModelIdx_; ///< Index of the actual joints in the pinocchio robot
std::vector<int32_t> rigidJointsPositionIdx_; ///< All the indices of the actual joints in the configuration vector of the robot (ie including all the degrees of freedom)
std::vector<int32_t> rigidJointsVelocityIdx_; ///< All the indices of the actual joints in the velocity vector of the robot (ie including all the degrees of freedom)
std::vector<std::string> flexibleJointsNames_; ///< Name of the flexibility joints of the robot regardless of whether the flexibilities are enable
std::vector<int32_t> flexibleJointsModelIdx_; ///< Index of the flexibility joints in the pinocchio robot regardless of whether the flexibilities are enable

vectorN_t positionLimitMin_; ///< Upper position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)

std::vector<std::string> positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the rigid robot
std::vector<std::string> velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the rigid robot
std::vector<std::string> accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the rigid robot

private:
pinocchio::Model pncModelFlexibleOrig_;
Expand Down
68 changes: 52 additions & 16 deletions core/src/engine/EngineMultiRobot.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -528,15 +528,30 @@ namespace jiminy
auto xInitIt = xInit.find(system.name);
if (xInitIt == xInit.end())
{
std::cout << "Error - EngineMultiRobot::start - At least one system does not have an initial state." << std::endl;
std::cout << "Error - EngineMultiRobot::start - At least one of the systems does not have an initial state." << std::endl;
returnCode = hresult_t::ERROR_BAD_INPUT;
}
if (returnCode == hresult_t::SUCCESS)
{
if (xInitIt->second.rows() != system.robot->nx())
{
std::cout << "Error - EngineMultiRobot::start - The size of the initial state is inconsistent "
"with model size for at least one system." << std::endl;
"with model size for at least one of the systems." << std::endl;
returnCode = hresult_t::ERROR_BAD_INPUT;
}
}
if (returnCode == hresult_t::SUCCESS)
{
auto const & qInit = xInitIt->second.head(system.robot->nq()).array();
auto const & vInit = xInitIt->second.tail(system.robot->nv()).array();

// Note that EPS allows to be very slightly out-of-bounds.
if ((EPS < qInit - system.robot->getPositionLimitMax().array()).any() ||
(EPS < system.robot->getPositionLimitMin().array() - qInit).any() ||
(EPS < vInit.abs() - system.robot->getVelocityLimit().array()).any())
{
std::cout << "Error - EngineMultiRobot::start - The initial state is out of bounds "
"for at least one of the systems." << std::endl;
returnCode = hresult_t::ERROR_BAD_INPUT;
}
}
Expand Down Expand Up @@ -691,11 +706,16 @@ namespace jiminy
forceMax = std::max(forceMax, fext.linear().norm());
}

auto const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx();
std::vector<int32_t> const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx();
std::vector<std::vector<int32_t> > const & collisionPairsIdx = system.robot->getCollisionPairsIdx();
for (uint32_t i=0; i < collisionBodiesIdx.size(); i++)
{
pinocchio::Force fext = computeContactDynamicsAtBody(system, i);
forceMax = std::max(forceMax, fext.linear().norm());
for (uint32_t j=0; j < collisionPairsIdx[i].size(); j++)
{
int32_t const & collisionPairIdx = collisionPairsIdx[i][j];
pinocchio::Force fext = computeContactDynamicsAtBody(system, collisionPairIdx);
forceMax = std::max(forceMax, fext.linear().norm());
}
}

if (forceMax > 1e5)
Expand Down Expand Up @@ -893,6 +913,8 @@ namespace jiminy

hresult_t EngineMultiRobot::step(float64_t stepSize)
{
hresult_t returnCode = hresult_t::SUCCESS;

// Check if the simulation has started
if (!isSimulationRunning_)
{
Expand Down Expand Up @@ -1011,7 +1033,7 @@ namespace jiminy
timer_.tic();

// Perform the integration. Do not simulate extremely small time steps.
while (tEnd - t > STEPPER_MIN_TIMESTEP)
while ((tEnd - t > STEPPER_MIN_TIMESTEP) && (returnCode == hresult_t::SUCCESS))
{
float64_t tNext = t;

Expand Down Expand Up @@ -1357,31 +1379,34 @@ namespace jiminy
{
std::cout << "Error - EngineMultiRobot::step - Too many successive iteration failures. "\
"Probably something is going wrong with the physics. Aborting integration." << std::endl;
return hresult_t::ERROR_GENERIC;
returnCode = hresult_t::ERROR_GENERIC;
}

if (dt < STEPPER_MIN_TIMESTEP)
{
std::cout << "Error - EngineMultiRobot::step - The internal time step is getting too small. "\
"Impossible to integrate physics further in time." << std::endl;
return hresult_t::ERROR_GENERIC;
returnCode = hresult_t::ERROR_GENERIC;
}

timer_.toc();
if (EPS < engineOptions_->stepper.timeout
&& engineOptions_->stepper.timeout < timer_.dt)
{
std::cout << "Error - EngineMultiRobot::step - Step computation timeout." << std::endl;
return hresult_t::ERROR_GENERIC;
returnCode = hresult_t::ERROR_GENERIC;
}
}

/* Update the final time and dt to make sure it corresponds
to the desired values and avoid compounding of error.
Anyway the user asked for a step of exactly stepSize,
so he is expecting this value to be reached. */
stepperState_.t = tEnd;
stepperState_.dt = stepSize;
if (returnCode == hresult_t::SUCCESS)
{
stepperState_.t = tEnd;
stepperState_.dt = stepSize;
}

/* Monitor current iteration number, and log the current time,
state, command, and sensors data. */
Expand All @@ -1390,7 +1415,12 @@ namespace jiminy
updateTelemetry();
}

return hresult_t::SUCCESS;
if (returnCode != hresult_t::SUCCESS)
{
stop();
}

return returnCode;
}

void EngineMultiRobot::stop(void)
Expand Down Expand Up @@ -1814,6 +1844,7 @@ namespace jiminy
{
pinocchio::forwardKinematics(system.robot->pncModel_, system.robot->pncData_, q, v, a);
pinocchio::updateFramePlacements(system.robot->pncModel_, system.robot->pncData_);
pinocchio::centerOfMass(system.robot->pncModel_, system.robot->pncData_);
pinocchio::updateGeometryPlacements(system.robot->pncModel_,
system.robot->pncData_,
system.robot->pncGeometryModel_,
Expand Down Expand Up @@ -2132,16 +2163,21 @@ namespace jiminy

// Compute the force at collision bodies
std::vector<int32_t> const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx();
std::vector<std::vector<int32_t> > const & collisionPairsIdx = system.robot->getCollisionPairsIdx();
for (uint32_t i=0; i < collisionBodiesIdx.size(); i++)
{
// Compute force at the given collision body.
// It returns the force applied at the origin of the parent joint frame, in global frame
int32_t const & frameIdx = collisionBodiesIdx[i];
pinocchio::Force const fextLocal = computeContactDynamicsAtBody(system, i);

// Apply the force at the origin of the parent joint frame, in local joint frame
int32_t const & parentJointIdx = system.robot->pncModel_.frames[frameIdx].parent;
fext[parentJointIdx] += fextLocal;
for (uint32_t j=0; j < collisionPairsIdx[i].size(); j++)
{
int32_t const & collisionPairIdx = collisionPairsIdx[i][j];
pinocchio::Force const fextLocal = computeContactDynamicsAtBody(system, collisionPairIdx);

// Apply the force at the origin of the parent joint frame, in local joint frame
fext[parentJointIdx] += fextLocal;
}
}

// Add the effect of user-defined external impulse forces
Expand Down
58 changes: 46 additions & 12 deletions core/src/robot/Model.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ namespace jiminy
collisionBodiesNames_(),
contactFramesNames_(),
collisionBodiesIdx_(),
collisionPairsIdx_(),
contactFramesIdx_(),
rigidJointsNames_(),
rigidJointsModelIdx_(),
Expand Down Expand Up @@ -270,15 +271,15 @@ namespace jiminy
pncGeometryModel_.addCollisionPair(collisionPair);
}
}

// Refresh proxies associated with the collisions only
refreshCollisionsProxies();
}

// Refresh proxies associated with the collisions only
refreshCollisionsProxies();

return hresult_t::SUCCESS;
}

hresult_t Model::removeCollisionBodies(std::vector<std::string> const & bodyNames)
hresult_t Model::removeCollisionBodies(std::vector<std::string> bodyNames)
{
if (!isInitialized_)
{
Expand All @@ -300,14 +301,25 @@ namespace jiminy
return hresult_t::ERROR_BAD_INPUT;
}

// Remove the list of bodies from the set of collision bodies
if (!bodyNames.empty())
/* Remove the list of bodies from the set of collision bodies, then
remove the associated set of collision pairs for each of them. */
if (bodyNames.empty())
{
eraseVector(collisionBodiesNames_, bodyNames);
bodyNames = collisionBodiesNames_;
}
else

for (uint32_t i=0; i<bodyNames.size(); ++i)
{
collisionBodiesNames_.clear();
std::string const & bodyName = bodyNames[i];
auto collisionBodiesNameIt = std::find(
collisionBodiesNames_.begin(),
collisionBodiesNames_.end(),
bodyName);
int32_t collisionBodiesNameIdx = std::distance(
collisionBodiesNames_.begin(),
collisionBodiesNameIt);
collisionBodiesNames_.erase(collisionBodiesNameIt);
collisionPairsIdx_.erase(collisionPairsIdx_.begin() + collisionBodiesNameIdx);
}

// Get the indices of the corresponding collision pairs in the geometry model of the robot and remove them
Expand All @@ -325,11 +337,11 @@ namespace jiminy
pncGeometryModel_.removeCollisionPair(collisionPair);
}
}

// Refresh proxies associated with the collisions only
refreshCollisionsProxies();
}

// Refresh proxies associated with the collisions only
refreshCollisionsProxies();

return hresult_t::SUCCESS;
}

Expand Down Expand Up @@ -753,6 +765,23 @@ namespace jiminy
pncGeometryData_->collisionRequest.num_max_contacts = mdlOptions_->collisions.maxContactPointsPerBody;
#endif

// Extract the indices of the collision pairs associated with each body
collisionPairsIdx_.clear();
for (std::string const & name : collisionBodiesNames_)
{
std::vector<int32_t> collisionPairsIdx;
for (uint32_t i=0; i<pncGeometryModel_.collisionPairs.size(); ++i)
{
pinocchio::CollisionPair const & pair = pncGeometryModel_.collisionPairs[i];
pinocchio::GeometryObject const & geom = pncGeometryModel_.geometryObjects[pair.first];
if (pncModel_.frames[geom.parentFrame].name == name)
{
collisionPairsIdx.push_back(i);
}
}
collisionPairsIdx_.push_back(std::move(collisionPairsIdx));
}

// Extract the contact frames indices in the model
getFramesIdx(pncModel_, collisionBodiesNames_, collisionBodiesIdx_);
}
Expand Down Expand Up @@ -1095,6 +1124,11 @@ namespace jiminy
return collisionBodiesIdx_;
}

std::vector<std::vector<int32_t> > const & Model::getCollisionPairsIdx(void) const
{
return collisionPairsIdx_;
}

std::vector<int32_t> const & Model::getContactFramesIdx(void) const
{
return contactFramesIdx_;
Expand Down
Loading