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

WIP: [core] Add transmission abstraction. #432

Draft
wants to merge 10 commits into
base: master
Choose a base branch
from
Draft

WIP: [core] Add transmission abstraction. #432

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
3047673
init commit transmission
Oct 29, 2021
06417c6
Merge branch 'master' into integrate_transmission
duburcqa Oct 29, 2021
52aae4e
update
Nov 8, 2021
500b569
update
Nov 9, 2021
b16bb60
update
Dec 8, 2021
6007e33
modifs
Dec 8, 2021
de296b4
comment all stuff to make it compile, put TODO
Dec 14, 2021
8ad2aa4
update
Dec 15, 2021
edec510
update
Dec 16, 2021
2b5dc0d
update
Dec 16, 2021
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
45 changes: 31 additions & 14 deletions core/include/jiminy/core/robot/AbstractTransmission.h
View file
Open in desktop
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;
fabinsch marked this conversation as resolved.
Show resolved Hide resolved

///////////////////////////////////////////////////////////////////////////////////////////////
/// \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,
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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 */
fabinsch marked this conversation as resolved.
Show resolved Hide resolved

///////////////////////////////////////////////////////////////////////////////////////////////
/// \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 */
fabinsch marked this conversation as resolved.
Show resolved Hide resolved

///////////////////////////////////////////////////////////////////////////////////////////////
/// \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_;
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
std::vector<int32_t> motorIndices_;
std::vector<std::weak_ptr<AbstractMotorBase> > motors_;
matrixN_t forwardTransform_;
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
matrixN_t backwardTransform_;

};
}

Expand Down
1 change: 1 addition & 0 deletions core/include/jiminy/core/robot/BasicMotors.h
View file
Open in desktop
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
Open in desktop
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*/,
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
Eigen::VectorBlock<vectorN_t const> const & /*v*/,
matrixN_t & out) final override;

virtual void computeInverseTransform(Eigen::VectorBlock<vectorN_t const> const & /*q*/,
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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);

fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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
Open in desktop
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
Open in desktop
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();
duburcqa marked this conversation as resolved.
Show resolved Hide resolved
if (system.robot->hasConstraints())
{
// Define some proxies for convenience
Expand Down
10 changes: 10 additions & 0 deletions core/src/robot/AbstractMotor.cc
View file
Open in desktop
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;
}
}


fabinsch marked this conversation as resolved.
Show resolved Hide resolved
if (returnCode == hresult_t::SUCCESS)
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
{
if (!robot->getIsInitialized())
Expand Down
39 changes: 36 additions & 3 deletions core/src/robot/AbstractTransmission.cc
View file
Open in desktop
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;
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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_
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,20 @@ namespace jiminy
setOptions(getDefaultMotorOptions());
}

hresult_t initialize(void)
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
{
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
Open in desktop
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.");
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
}
}

void computeEffortTransmission(void)
{
// TODO
}
}
10 changes: 5 additions & 5 deletions python/jiminy_pywrap/src/Motors.cc
View file
Open in desktop
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
Open in desktop
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