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 edec510 commit 47b72b9
Show file tree
Hide file tree
Showing 9 changed files with 111 additions and 98 deletions.
6 changes: 4 additions & 2 deletions core/include/jiminy/core/robot/AbstractTransmission.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -232,10 +232,11 @@ namespace jiminy
///
/// \param[in] q Current configuration of the motors.
/// \param[in] v Current velocity of the motors.
/// \param[out] out Matrix transforming quantities from motor to joint level
///////////////////////////////////////////////////////////////////////////////////////////////
virtual void computeTransform(Eigen::VectorBlock<vectorN_t const> const & q,
Eigen::VectorBlock<vectorN_t const> const & v,
matrixN_t & out) = 0; /* copy on purpose */
matrixN_t & out) = 0;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Request the transmission to update its actual state based of the input data.
Expand All @@ -245,10 +246,11 @@ namespace jiminy
///
/// \param[in] q Current configuration of the motors.
/// \param[in] v Current velocity of the motors.
/// \param[out] out Matrix transforming quantities from joint to motor level
///////////////////////////////////////////////////////////////////////////////////////////////
virtual void computeInverseTransform(Eigen::VectorBlock<vectorN_t const> const & q,
Eigen::VectorBlock<vectorN_t const> const & v,
matrixN_t & out) = 0; /* copy on purpose */
matrixN_t & out) = 0;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute energy dissipation in the transmission.
Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/robot/BasicMotors.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ namespace jiminy
virtual hresult_t setOptions(configHolder_t const & motorOptions) final override;

private:
virtual hresult_t computeEffort(float64_t command) final override;
virtual hresult_t computeEffort(float64_t command) final override; /* copy on purpose */

private:
std::unique_ptr<motorOptions_t const> motorOptions_;
Expand Down
8 changes: 4 additions & 4 deletions core/include/jiminy/core/robot/BasicTransmissions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,12 @@ 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*/,
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*/,
virtual void computeInverseTransform(Eigen::VectorBlock<vectorN_t const> const & q,
Eigen::VectorBlock<vectorN_t const> const & v,
matrixN_t & out) final override;

virtual void computeEffortTransmission(void);
Expand Down
1 change: 0 additions & 1 deletion core/src/robot/AbstractMotor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,7 +240,6 @@ namespace jiminy
}
}


if (returnCode == hresult_t::SUCCESS)
{
if (!robot->getIsInitialized())
Expand Down
172 changes: 95 additions & 77 deletions core/src/robot/AbstractTransmission.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,70 +52,6 @@ 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;
for (std::string const & jointName : jointNames_)
{
std::vector<int32_t> jointPositionIdx;
if (!robot->pncModel_.existJointName(jointName))
{
PRINT_ERROR("Joint '", jointName, "' not found in robot model.");
return hresult_t::ERROR_BAD_INPUT;
}
if (returnCode == hresult_t::SUCCESS)
{
returnCode = getJointPositionIdx(robot->pncModel_, jointName, jointPositionIdx);
}
if (returnCode == hresult_t::SUCCESS)
{
jointPositionIndices.insert(jointPositionIndices.end(), jointPositionIdx.begin(), jointPositionIdx.end());
}
}
int32_t jointPositionSize = jointPositionIndices.size();
jointPositionIndices_.resize(jointPositionSize);
for (int32_t i = 0; i < jointPositionSize; ++i)
{
jointPositionIndices_(i) = jointPositionIndices[i];
}

// Populate jointVelocityIndices_
std::vector<int32_t> jointVelocityIndices;
for (std::string const & jointName : jointNames_)
{
std::vector<int32_t> jointVelocityIdx;
if (!robot->pncModel_.existJointName(jointName))
{
PRINT_ERROR("Joint '", jointName, "' not found in robot model.");
return hresult_t::ERROR_BAD_INPUT;
}
jointIndex_t const & jointModelIdx = robot->pncModel_.getJointId(jointName);
int32_t const & jointVelocityFirstIdx = robot->pncModel_.joints[jointModelIdx].idx_v();
int32_t const & jointNv = robot->pncModel_.joints[jointModelIdx].nv();
jointVelocityIdx.resize(jointNv);
std::iota(jointVelocityIdx.begin(), jointVelocityIdx.end(), jointVelocityFirstIdx);
jointVelocityIndices.insert(jointVelocityIndices.end(), jointVelocityIdx.begin(), jointVelocityIdx.end());
}

int32_t jointVelocitySize = jointVelocityIndices.size();
jointVelocityIndices_.resize(jointVelocitySize);
for (int32_t i = 0; i < jointVelocitySize; ++i)
{
jointVelocityIndices_(i) = jointVelocityIndices[i];
}
return returnCode;
}

Expand Down Expand Up @@ -287,6 +223,71 @@ namespace jiminy
}
}

// Populate motorIndices_
std::weak_ptr<AbstractMotorBase const> motor;
for (std::string const & motorName : getMotorNames())
{
if (returnCode == hresult_t::SUCCESS)
{
returnCode = robot->getMotor(motorName, motor);
auto motorTemp = motor.lock();
int32_t idx = motorTemp->getIdx();
motorIndices_.push_back(idx);
}
}

// PopulatjointPositionIndices_
std::vector<int32_t> jointPositionIndices;
for (std::string const & jointName : jointNames_)
{
std::vector<int32_t> jointPositionIdx;
if (!robot->pncModel_.existJointName(jointName))
{
PRINT_ERROR("Joint '", jointName, "' not found in robot model.");
return hresult_t::ERROR_BAD_INPUT;
}
if (returnCode == hresult_t::SUCCESS)
{
returnCode = getJointPositionIdx(robot->pncModel_, jointName, jointPositionIdx);
}
if (returnCode == hresult_t::SUCCESS)
{
jointPositionIndices.insert(jointPositionIndices.end(), jointPositionIdx.begin(), jointPositionIdx.end());
}
}
int32_t jointPositionSize = jointPositionIndices.size();
jointPositionIndices_.resize(jointPositionSize);
for (int32_t i = 0; i < jointPositionSize; ++i)
{
jointPositionIndices_(i) = jointPositionIndices[i];
}


// Populate jointVelocityIndices_
std::vector<int32_t> jointVelocityIndices;
for (std::string const & jointName : jointNames_)
{
std::vector<int32_t> jointVelocityIdx;
if (!robot->pncModel_.existJointName(jointName))
{
PRINT_ERROR("Joint '", jointName, "' not found in robot model.");
return hresult_t::ERROR_BAD_INPUT;
}
jointIndex_t const & jointModelIdx = robot->pncModel_.getJointId(jointName);
int32_t const & jointVelocityFirstIdx = robot->pncModel_.joints[jointModelIdx].idx_v();
int32_t const & jointNv = robot->pncModel_.joints[jointModelIdx].nv();
jointVelocityIdx.resize(jointNv);
std::iota(jointVelocityIdx.begin(), jointVelocityIdx.end(), jointVelocityFirstIdx);
jointVelocityIndices.insert(jointVelocityIndices.end(), jointVelocityIdx.begin(), jointVelocityIdx.end());
}

int32_t jointVelocitySize = jointVelocityIndices.size();
jointVelocityIndices_.resize(jointVelocitySize);
for (int32_t i = 0; i < jointVelocitySize; ++i)
{
jointVelocityIndices_(i) = jointVelocityIndices[i];
}

return returnCode;
}

Expand Down Expand Up @@ -337,42 +338,56 @@ namespace jiminy
return motorNames_;
}

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

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

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

hresult_t AbstractTransmissionBase::computeForward(float64_t const & t,
hresult_t AbstractTransmissionBase::computeForward(float64_t const & /*t*/,
vectorN_t & q,
vectorN_t & v,
vectorN_t & a,
vectorN_t & uJoint)
{
// Extract motor configuration and velocity from all motors attached
// to the robot for this transmission
auto qMotors = q.segment<>(jointPositionIndices_, );
auto vMotors = v.segment<>(jointVelocityIndices_, );
auto qMotors = q.segment(jointPositionIndices_);
auto vMotors = v.segment(jointVelocityIndices_);

// Compute the transmission effect based on the current configuration
computeTransform(qMotors, vMotors);
computeTransform(qMotors, vMotors, forwardTransform_);

// Gather the corresponding data of the motors attached to the transmission
int32_t numberOfMotors = motors_.size();
vectorN_t qTemp(numberOfMotors);
vectorN_t vTemp(numberOfMotors);
vectorN_t aTemp(numberOfMotors);
vectorN_t uTemp(numberOfMotors);

for (int32_t i = 0; i < numberOfMotors; i++)
{
auto motorTemp = motors_[i].lock();
qTemp[i] = motorTemp->getPosition();
vTemp[i] = motorTemp->getVelocity();
aTemp[i] = motorTemp->getAcceleration();
uTemp[i] = motorTemp->getEffort();
}

// Apply transformation from motor to joint level
auto motors = motors_.lock();
q.noalias() = forwardTransform_ * motors->getPosition();
v.noalias() = forwardTransform_ * motors->getVelocity();
a.noalias() = forwardTransform_ * motors->getAcceleration();
uJoint.noalias() = forwardTransform_ * motors->getEffort();
q.noalias() = forwardTransform_ * qTemp;
v.noalias() = forwardTransform_ * vTemp;
a.noalias() = forwardTransform_ * aTemp;
uJoint.noalias() = forwardTransform_ * uTemp;
}

hresult_t AbstractTransmissionBase::computeBackward(float64_t const & t,
Expand All @@ -381,7 +396,10 @@ namespace jiminy
vectorN_t const & a,
vectorN_t const & uJoint)
{
// Compute the transmission effect based on the current configuration
computeInverseTransform(q, v);

// TODO similar way than forward
auto motors = motors_.lock();
motors->q = backwardTransform_ * q;
motors->v = backwardTransform_ * v;
Expand Down
2 changes: 1 addition & 1 deletion core/src/robot/BasicMotors.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ namespace jiminy
setOptions(getDefaultMotorOptions());
}

hresult_t initialize(void)
hresult_t SimpleMotor::initialize(void)
{
hresult_t returnCode = hresult_t::SUCCESS;
isInitialized_ = true;
Expand Down
4 changes: 2 additions & 2 deletions core/src/robot/BasicTransmissions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ namespace jiminy
{
if (!isInitialized_)
{
PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort.");
PRINT_ERROR("Transmission not initialized. Impossible to compute transformation of transmission.");
}
}

Expand All @@ -33,7 +33,7 @@ namespace jiminy
{
if (!isInitialized_)
{
PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort.");
PRINT_ERROR("Transmission not initialized. Impossible to compute transformation of transmission.");
}
}

Expand Down
10 changes: 0 additions & 10 deletions python/jiminy_pywrap/src/Motors.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,16 +32,6 @@ namespace python
bp::return_value_policy<bp::copy_const_reference>()))
.add_property("idx", bp::make_function(&AbstractMotorBase::getIdx,
bp::return_value_policy<bp::copy_const_reference>()))
// .add_property("joint_name", bp::make_function(&AbstractMotorBase::getJointName,
// bp::return_value_policy<bp::copy_const_reference>()))
// .add_property("joint_idx", bp::make_function(&AbstractMotorBase::getJointModelIdx,
// bp::return_value_policy<bp::copy_const_reference>()))
// .add_property("joint_type", bp::make_function(&AbstractMotorBase::getJointType,
// bp::return_value_policy<bp::copy_const_reference>()))
// .add_property("joint_position_idx", bp::make_function(&AbstractMotorBase::getJointPositionIdx,
// bp::return_value_policy<bp::copy_const_reference>()))
// .add_property("joint_velocity_idx", bp::make_function(&AbstractMotorBase::getJointVelocityIdx,
// bp::return_value_policy<bp::copy_const_reference>()))
.add_property("command_limit", bp::make_function(&AbstractMotorBase::getCommandLimit,
bp::return_value_policy<bp::copy_const_reference>()))
.add_property("armature", bp::make_function(&AbstractMotorBase::getArmature,
Expand Down
4 changes: 4 additions & 0 deletions python/jiminy_pywrap/src/Transmissions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,10 @@ namespace python
bp::return_value_policy<bp::copy_const_reference>()))
.add_property("joint_velocity_indices", bp::make_function(&AbstractTransmissionBase::getJointVelocityIndices,
bp::return_value_policy<bp::copy_const_reference>()))
.add_property("forward_transform", bp::make_function(&AbstractTransmissionBase::getForward,
bp::return_value_policy<result_converter<false> >()))
.add_property("backward_transform", bp::make_function(&AbstractTransmissionBase::getBackward,
bp::return_value_policy<result_converter<false> >()))

.def("set_options", &PyAbstractTransmissionVisitor::setOptions)
.def("get_options", &AbstractTransmissionBase::getOptions)
Expand Down

0 comments on commit 47b72b9

Please sign in to comment.