Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history
  • Loading branch information
fabians committed Dec 16, 2021
1 parent 8ad2aa4 commit edec510
Show file tree
Hide file tree
Showing 11 changed files with 116 additions and 31 deletions.
45 changes: 31 additions & 14 deletions core/include/jiminy/core/robot/AbstractTransmission.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,11 +101,6 @@ namespace jiminy
///////////////////////////////////////////////////////////////////////////////////////////////
configHolder_t getOptions(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the actual state of the transmission at the current time.
///////////////////////////////////////////////////////////////////////////////////////////////
float64_t const & get(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Set the configuration options of the transmission.
///
Expand Down Expand Up @@ -177,33 +172,54 @@ namespace jiminy
vectorN_t const & getJointVelocityIndices(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get motorName_.
/// \brief Get motorNames_.
///
/// \details It is the name of the motors associated with the transmission.
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<std::string> const & getMotorNames(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get getMotorIndices.
///
/// \details It is the name of the motors associated with the transmission.
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<int32_t> const & getMotorIndices(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get forwardTransform_.
///
/// \details It is the transformation matrix to convert quantities from motor to joint-level.
///////////////////////////////////////////////////////////////////////////////////////////////
matrixN_t const & getForwardTransform(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get backwardTransform_.
///
/// \details It is the transformation matrix to convert quantities from joint to motor-level.
///////////////////////////////////////////////////////////////////////////////////////////////
matrixN_t const & getInverseTransform(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute forward transmission.
///
/// \details Compute forward transmission from motor to joint.
///////////////////////////////////////////////////////////////////////////////////////////////
virtual hresult_t computeForward(float64_t const & t,
hresult_t computeForward(float64_t const & t,
vectorN_t & q,
vectorN_t & v,
vectorN_t & a,
vectorN_t & uJoint) final;
vectorN_t & uJoint);

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute backward transmission.
///
/// \details Compute backward transmission from joint to motor.
///////////////////////////////////////////////////////////////////////////////////////////////
virtual hresult_t computeBackward(float64_t const & t,
hresult_t computeBackward(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t const & a,
vectorN_t const & uJoint) final;
vectorN_t const & uJoint);

protected:
///////////////////////////////////////////////////////////////////////////////////////////////
Expand All @@ -218,7 +234,8 @@ namespace jiminy
/// \param[in] v Current velocity of the motors.
///////////////////////////////////////////////////////////////////////////////////////////////
virtual void computeTransform(Eigen::VectorBlock<vectorN_t const> const & q,
Eigen::VectorBlock<vectorN_t const> const & v) = 0; /* copy on purpose */
Eigen::VectorBlock<vectorN_t const> const & v,
matrixN_t & out) = 0; /* copy on purpose */

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Request the transmission to update its actual state based of the input data.
Expand All @@ -230,8 +247,8 @@ namespace jiminy
/// \param[in] v Current velocity of the motors.
///////////////////////////////////////////////////////////////////////////////////////////////
virtual void computeInverseTransform(Eigen::VectorBlock<vectorN_t const> const & q,
Eigen::VectorBlock<vectorN_t const> const & v) = 0; /* copy on purpose */

Eigen::VectorBlock<vectorN_t const> const & v,
matrixN_t & out) = 0; /* copy on purpose */

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute energy dissipation in the transmission.
Expand Down Expand Up @@ -268,10 +285,10 @@ namespace jiminy
vectorN_t jointPositionIndices_;
vectorN_t jointVelocityIndices_;
std::vector<std::string> motorNames_;
std::vector<int32_t> motorIndices_;
std::vector<std::weak_ptr<AbstractMotorBase> > motors_;
matrixN_t forwardTransform_;
matrixN_t backwardTransform_;

};
}

Expand Down
1 change: 1 addition & 0 deletions core/include/jiminy/core/robot/BasicMotors.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ namespace jiminy
auto shared_from_this() { return shared_from(this); }
auto shared_from_this() const { return shared_from(this); }

hresult_t initialize(void);
virtual hresult_t setOptions(configHolder_t const & motorOptions) final override;

private:
Expand Down
10 changes: 7 additions & 3 deletions core/include/jiminy/core/robot/BasicTransmissions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,15 @@ namespace jiminy
virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override;

private:
virtual void computeTransform(Eigen::VectorBlock<vectorN_t const> const & /*q*/ ,
Eigen::VectorBlock<vectorN_t const> const & /*v*/) final override;
virtual void computeTransform(Eigen::VectorBlock<vectorN_t const> const & /*q*/,
Eigen::VectorBlock<vectorN_t const> const & /*v*/,
matrixN_t & out) final override;

virtual void computeInverseTransform(Eigen::VectorBlock<vectorN_t const> const & /*q*/,
Eigen::VectorBlock<vectorN_t const> const & /*v*/) final override;
Eigen::VectorBlock<vectorN_t const> const & /*v*/,
matrixN_t & out) final override;

virtual void computeEffortTransmission(void);

private:
std::unique_ptr<transmissionOptions_t const> transmissionOptions_;
Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/robot/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ namespace jiminy
vectorN_t const & command);
vectorN_t const & getMotorsEfforts(void) const;
float64_t const & getMotorEffort(std::string const & motorName) const;
float64_t getMotorEffortLimit(void);
vectorN_t const & getMotorsEffortLimits(void) const;
void setSensorsData(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
Expand Down
3 changes: 1 addition & 2 deletions core/src/engine/EngineMultiRobot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3687,8 +3687,7 @@ namespace jiminy
pinocchio::Model const & model = system.robot->pncModel_;
pinocchio::Data & data = system.robot->pncData_;

// TODO wtf
// data.rotorInertia = system.robot->getArmature();
data.rotorInertia = system.robot->getArmatures();
if (system.robot->hasConstraints())
{
// Define some proxies for convenience
Expand Down
10 changes: 10 additions & 0 deletions core/src/robot/AbstractMotor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -231,6 +231,16 @@ namespace jiminy
}
}

if (returnCode == hresult_t::SUCCESS)
{
if (!isInitialized_)
{
PRINT_ERROR("Motor not initialized. Impossible to refresh proxies.");
returnCode = hresult_t::ERROR_INIT_FAILED;
}
}


if (returnCode == hresult_t::SUCCESS)
{
if (!robot->getIsInitialized())
Expand Down
39 changes: 36 additions & 3 deletions core/src/robot/AbstractTransmission.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,22 @@ namespace jiminy
isInitialized_ = false;
}

auto robot = robot_.lock();

// TODO move this stuff to refresh Proxies ?
// Populate motorIndices_
std::weak_ptr<AbstractMotorBase const> motor;
for (std::string const & motorName : motorNames)
{
returnCode = robot->getMotor(motorName, motor);
auto motorTemp = motor.lock();
int32_t idx = motorTemp->getIdx();
motorIndices_.push_back(idx);
}

// Populate jointPositionIndices_
std::vector<int32_t> jointPositionIndices;
hresult_t returnCode = hresult_t::SUCCESS;
auto robot = robot_.lock();
for (std::string const & jointName : jointNames_)
{
std::vector<int32_t> jointPositionIdx;
Expand Down Expand Up @@ -98,7 +110,12 @@ namespace jiminy
jointVelocityIndices.insert(jointVelocityIndices.end(), jointVelocityIdx.begin(), jointVelocityIdx.end());
}

// missing how to populate vectorN_t jointVelocityIndices_
int32_t jointVelocitySize = jointVelocityIndices.size();
jointVelocityIndices_.resize(jointVelocitySize);
for (int32_t i = 0; i < jointVelocitySize; ++i)
{
jointVelocityIndices_(i) = jointVelocityIndices[i];
}
return returnCode;
}

Expand All @@ -118,7 +135,7 @@ namespace jiminy
return hresult_t::ERROR_GENERIC;
}

// Make sure the joint is not already attached to a transmission
// TODO Make sure the joint is not already attached to a transmission
// WARNING at this point it is still not know which joint or motor the transmision connects
// auto robotTemp = robot.lock();
// std::vector<std::string> actuatedJointNames = robotTemp->getActuatedJointNames();
Expand Down Expand Up @@ -320,6 +337,22 @@ namespace jiminy
return motorNames_;
}

std::vector<std::string> const & getMotorIndices(void) const
{
// TODO create and populate
return motorIndices_;
}

matrixN_t const & getForwardTransform(void) const
{
return forwardTransform_;
}

matrixN_t const & getInverseTransform(void) const
{
return backwardTransform_;
}

hresult_t AbstractTransmissionBase::computeForward(float64_t const & t,
vectorN_t & q,
vectorN_t & v,
Expand Down
14 changes: 14 additions & 0 deletions core/src/robot/BasicMotors.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,20 @@ namespace jiminy
setOptions(getDefaultMotorOptions());
}

hresult_t initialize(void)
{
hresult_t returnCode = hresult_t::SUCCESS;
isInitialized_ = true;
returnCode = refreshProxies();

if (returnCode != hresult_t::SUCCESS)
{
isInitialized_ = false;
}

return returnCode;
}

hresult_t SimpleMotor::setOptions(configHolder_t const & motorOptions)
{
hresult_t returnCode = hresult_t::SUCCESS;
Expand Down
11 changes: 9 additions & 2 deletions core/src/robot/BasicTransmissions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,8 @@ namespace jiminy
}

void SimpleTransmission::computeTransform(Eigen::VectorBlock<vectorN_t const> const & /*q*/,
Eigen::VectorBlock<vectorN_t const> const & /*v*/)
Eigen::VectorBlock<vectorN_t const> const & /*v*/,
matrixN_t &out)
{
if (!isInitialized_)
{
Expand All @@ -27,11 +28,17 @@ namespace jiminy
}

void SimpleTransmission::computeInverseTransform(Eigen::VectorBlock<vectorN_t const> const & /*q*/,
Eigen::VectorBlock<vectorN_t const> const & /*v*/)
Eigen::VectorBlock<vectorN_t const> const & /*v*/,
matrixN_t &out)
{
if (!isInitialized_)
{
PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort.");
}
}

void computeEffortTransmission(void)
{
// TODO
}
}
10 changes: 5 additions & 5 deletions python/jiminy_pywrap/src/Motors.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,8 @@ namespace python
void visit(PyClass & cl) const
{
cl
// .add_property("is_initialized", bp::make_function(&AbstractMotorBase::getIsInitialized,
// bp::return_value_policy<bp::copy_const_reference>()))
.add_property("is_initialized", bp::make_function(&AbstractMotorBase::getIsInitialized,
bp::return_value_policy<bp::copy_const_reference>()))
.add_property("name", bp::make_function(&AbstractMotorBase::getName,
bp::return_value_policy<bp::copy_const_reference>()))
.add_property("idx", bp::make_function(&AbstractMotorBase::getIdx,
Expand Down Expand Up @@ -93,9 +93,9 @@ namespace python

static void visit(PyClass & cl)
{
// cl
// .def("initialize", &TMotor::initialize)
// ;
cl
.def("initialize", &TMotor::initialize)
;
}
};

Expand Down
2 changes: 1 addition & 1 deletion python/jiminy_pywrap/src/Transmissions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ namespace python
std::shared_ptr<SimpleTransmission>,
boost::noncopyable>("SimpleTransmission",
bp::init<std::string const &>(
bp::args("self", "Transmission_name")))
bp::args("self", "transmission_name")))
.def(PySimpleTransmissionVisitor());
}
};
Expand Down

0 comments on commit edec510

Please sign in to comment.