Skip to content
This repository has been archived by the owner on Dec 1, 2020. It is now read-only.

Feature/pm 524 Generalize motor controller #284

Draft
wants to merge 35 commits into
base: develop
Choose a base branch
from
Draft
Changes from 1 commit
Commits
Show all changes
35 commits
Select commit Hold shift + click to select a range
6abd775
Create MotorController abstract class
Roelemans Jul 16, 2020
c8f3af5
Merge branch 'develop' into feature/PM-524-add-odrive-option
Roelemans Aug 5, 2020
f07ebf8
Have Joint.cpp use controller instead of imc
Roelemans Aug 6, 2020
1db4b01
Generalise hardware_builder, hardware_interface and ehtercat master a…
Roelemans Aug 7, 2020
7cf1336
Fix tests #1
Roelemans Aug 7, 2020
9d320e5
Fix tests #2
Roelemans Aug 8, 2020
fffab68
Throw a fatal when a joint has no controller defined and remove runti…
Roelemans Aug 8, 2020
ae33d44
Fix error msg
Roelemans Aug 11, 2020
00e9bad
Fix typos
Roelemans Aug 11, 2020
2e1b2f4
Merge branch 'develop' into feature/PM-524-add-odrive-option
Roelemans Aug 11, 2020
ef553b9
Namechange MotorControllerState to MotorControllerStates
Roelemans Aug 11, 2020
a60801f
Fix clang
Roelemans Aug 11, 2020
087227e
Remove tests untill it works
Roelemans Aug 12, 2020
9d8bc65
Fix build
Roelemans Aug 12, 2020
81423e5
Generalise imc_state change some naming and add docstrings
Roelemans Aug 13, 2020
5e73645
Moved checkState and getErrorStatus from MotorController to MotorCont…
Roelemans Aug 13, 2020
f0c296f
Clang
Roelemans Aug 13, 2020
ea89e8e
Reduce ethercat dependency in naming
Roelemans Aug 14, 2020
03c994a
Merge branch 'develop' into feature/PM-524-add-odrive-option
Roelemans Aug 14, 2020
df36bbf
Mock motor controller and fix bugs
Roelemans Aug 14, 2020
5edecb5
Revert generalise ethercat naming
Roelemans Aug 21, 2020
58fbd2f
Use Ampere instead of IU for torque, and change input/return type of …
Roelemans Aug 21, 2020
855e436
Rename variable in header
Roelemans Aug 21, 2020
e68a574
Return smart pointer instead of reference and minor fixes
Roelemans Aug 24, 2020
0a1c16a
Return smart pointer instead of reference and minor fixes #2
Roelemans Aug 24, 2020
c49c6b3
Merge branch 'develop' into feature/PM-524-add-odrive-option
Roelemans Aug 25, 2020
0fbe358
Create slave_list object
Roelemans Aug 25, 2020
4f66858
Pass a slave list to ethercat master
Roelemans Aug 25, 2020
b48674d
Fix tests
Roelemans Aug 25, 2020
81f0db4
Fix tests
Roelemans Aug 25, 2020
41257c8
Remove initialize from Joint and MotorController
Roelemans Aug 25, 2020
79a93a3
Move hasValidSlaves() to ethercat master
Roelemans Aug 25, 2020
1e55adb
Remove getSlaveIndex from MotorController
Roelemans Aug 25, 2020
ea171c0
Actually use make_shared for shared_ptrs
Roelemans Aug 26, 2020
9b5cc5c
Add docstring
Roelemans Aug 26, 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
Prev Previous commit
Next Next commit
Use Ampere instead of IU for torque, and change input/return type of …
…some methods
Roelemans committed Aug 21, 2020
commit 58fbd2f16ae3eb2d217514f939800b260d61cc6b
2 changes: 1 addition & 1 deletion march_hardware/include/march_hardware/ethercat/slave.h
Original file line number Diff line number Diff line change
@@ -31,7 +31,7 @@ class Slave : public PdoSlaveInterface

~Slave() noexcept override = default;

uint16_t getSlaveIndex() const
int getSlaveIndex() const
Roelemans marked this conversation as resolved.
Show resolved Hide resolved
{
return this->slave_index_;
}
8 changes: 2 additions & 6 deletions march_hardware/include/march_hardware/joint.h
Original file line number Diff line number Diff line change
@@ -50,19 +50,15 @@ class Joint
void prepareActuation();

void actuateRad(double target_position);
void actuateTorque(int16_t target_torque);
void actuateTorque(double target_torque);
void readEncoders(const ros::Duration& elapsed_time);

double getPosition() const;
double getVelocity() const;
double getVoltageVelocity() const;
double getIncrementalPosition() const;
double getAbsolutePosition() const;
int16_t getTorque();
int32_t getAngleIUAbsolute();
int32_t getAngleIUIncremental();
double getVelocityIUAbsolute();
double getVelocityIUIncremental();
double getTorque();
float getTemperature();
MotorControllerStates& getMotorControllerStates();

Original file line number Diff line number Diff line change
@@ -48,7 +48,7 @@ class IMotionCube : public MotorController, public Slave
double getAbsoluteRadPerBit() const;
double getIncrementalRadPerBit() const;
bool getIncrementalMorePrecise() const override;
int16_t getTorque() override;
double getTorque() override;
int32_t getAngleIUAbsolute();
int32_t getAngleIUIncremental();
double getVelocityIUAbsolute();
@@ -71,13 +71,13 @@ class IMotionCube : public MotorController, public Slave

void setControlWord(uint16_t control_word);
virtual void actuateRad(double target_rad) override;
virtual void actuateTorque(int16_t target_torque) override;
virtual void actuateTorque(double target_torque) override;

bool initialize(int cycle_time) override;
void goToTargetState(IMotionCubeTargetState target_state);
virtual void prepareActuation() override;

uint16_t getSlaveIndex() const override;
int getSlaveIndex() const override;
virtual void reset() override;

/** @brief Override comparison operator */
@@ -97,7 +97,9 @@ class IMotionCube : public MotorController, public Slave
constexpr static double MAX_TARGET_DIFFERENCE = 0.393;
// This value is slightly larger than the current limit of the
// linear joints defined in the URDF.
const static int16_t MAX_TARGET_TORQUE = 23500;
constexpr static double IPEAK = 40;
// See CoE manual page 222
constexpr static double AMPERE_TO_IU_FACTOR = 65520;

// Watchdog base time = 1 / 25 MHz * (2498 + 2) = 0.0001 seconds=100 µs
static const uint16_t WATCHDOG_DIVIDER = 2498;
@@ -111,6 +113,7 @@ class IMotionCube : public MotorController, public Slave

private:
void actuateIU(int32_t target_iu);
int16_t ampereToTorqueIU(double ampere);

void mapMisoPDOs(SdoSlaveInterface& sdo);
void mapMosiPDOs(SdoSlaveInterface& sdo);
Original file line number Diff line number Diff line change
@@ -15,15 +15,15 @@ class MotorController
virtual double getAngleRadIncremental() = 0;
virtual double getVelocityRadAbsolute() = 0;
virtual double getVelocityRadIncremental() = 0;
virtual int16_t getTorque() = 0;
virtual double getTorque() = 0;

virtual ActuationMode getActuationMode() const = 0;

/**
* Getter for the slave index.
* @return slave index if the motor controller is an ethercat slave, else -1
*/
virtual uint16_t getSlaveIndex() const = 0;
virtual int getSlaveIndex() const = 0;

virtual float getMotorCurrent() = 0;
virtual float getMotorControllerVoltage() = 0;
@@ -36,7 +36,7 @@ class MotorController
virtual bool getIncrementalMorePrecise() const = 0;

virtual void actuateRad(double target_rad) = 0;
virtual void actuateTorque(int16_t target_torque) = 0;
virtual void actuateTorque(double target_torque_ampere) = 0;

virtual void prepareActuation() = 0;
virtual bool initialize(int cycle_time) = 0;
25 changes: 16 additions & 9 deletions march_hardware/src/imotioncube/imotioncube.cpp
Original file line number Diff line number Diff line change
@@ -184,29 +184,30 @@ void IMotionCube::actuateIU(int32_t target_iu)
this->write32(target_position_location, target_position);
}

void IMotionCube::actuateTorque(int16_t target_torque)
void IMotionCube::actuateTorque(double target_torque_ampere)
{
if (target_torque_ampere >= IPEAK)
{
throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE,
"Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK);
}
int16_t target_torque = ampereToTorqueIU(target_torque_ampere);

if (this->actuation_mode_ != ActuationMode::torque)
{
throw error::HardwareException(error::ErrorType::INVALID_ACTUATION_MODE,
"trying to actuate torque, while actuation mode is %s",
this->actuation_mode_.toString().c_str());
}

if (target_torque >= MAX_TARGET_TORQUE)
{
throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE,
"Target torque of %d exceeds max torque of %d", target_torque, MAX_TARGET_TORQUE);
}

bit16 target_torque_struct = { .i = target_torque };

uint8_t target_torque_location = this->mosi_byte_offsets_.at(IMCObjectName::TargetTorque);

this->write16(target_torque_location, target_torque_struct);
}

uint16_t IMotionCube::getSlaveIndex() const
int IMotionCube::getSlaveIndex() const
{
return this->Slave::getSlaveIndex();
}
@@ -246,7 +247,7 @@ double IMotionCube::getIncrementalRadPerBit() const
return this->incremental_encoder_->getRadPerBit();
}

int16_t IMotionCube::getTorque()
double IMotionCube::getTorque()
{
bit16 return_byte = this->read16(this->miso_byte_offsets_.at(IMCObjectName::ActualTorque));
return return_byte.i;
@@ -554,4 +555,10 @@ ActuationMode IMotionCube::getActuationMode() const
{
return this->actuation_mode_;
}

int16_t IMotionCube::ampereToTorqueIU(double ampere)
{
// See CoE manual page 222
return AMPERE_TO_IU_FACTOR * ampere / (2 * IPEAK);
}
} // namespace march
4 changes: 2 additions & 2 deletions march_hardware/src/joint.cpp
Original file line number Diff line number Diff line change
@@ -141,7 +141,7 @@ double Joint::getAbsolutePosition() const
return this->absolute_position_;
}

void Joint::actuateTorque(int16_t target_torque)
void Joint::actuateTorque(double target_torque)
{
if (!this->canActuate())
{
@@ -151,7 +151,7 @@ void Joint::actuateTorque(int16_t target_torque)
this->controller_->actuateTorque(target_torque);
}

int16_t Joint::getTorque()
double Joint::getTorque()
{
return this->controller_->getTorque();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This will end horribly, if the controller_ is a nullptr, always check you pointers before using them. I now see you do this a lot more, so be warned

Copy link
Contributor Author

@Roelemans Roelemans Aug 12, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I build a check in hardware builder to ensure that every joint has a controller at startup, removing the need to check this every time during runtime.

PS previously, the check at startup was not there, while not everyone of the methods in joint did the nullptr check (e.g. Joint.getIMotionCubeState), so this should actually be an improvement

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What if the hardware is not initialized by the hardware builder, what if it contains a bug that results in creating a nullptr? You can't assume that a check somewhere else will protect this class. I know not every method contained the check, but that is not an argument for completely removing it.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm do you think a check in the constructor suffice? That will also immediately cover for all the plebs that appearantly like to make methods without these checks. Also, it will make the code less bloated

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The constructor would only check this once. Adding a check here would ensure that it is always a valid pointer.

}
8 changes: 4 additions & 4 deletions march_hardware/src/march_robot.cpp
Original file line number Diff line number Diff line change
@@ -48,17 +48,17 @@ void MarchRobot::startEtherCAT(bool reset_motor_controllers)
return;
}

bool config_write_reset = ethercatMaster.start(this->jointList);
bool config_overwritten = ethercatMaster.start(this->jointList);

if (reset_motor_controllers || config_write_reset)
if (reset_motor_controllers || config_overwritten)
{
ROS_DEBUG("Resetting all motor controllers due to either: reset arg: %d or downloading of configuration file: %d",
reset_motor_controllers, config_write_reset);
reset_motor_controllers, config_overwritten);
resetMotorControllers();

ROS_INFO("Restarting the EtherCAT Master");
ethercatMaster.stop();
config_write_reset = ethercatMaster.start(this->jointList);
config_overwritten = ethercatMaster.start(this->jointList);
}
}

6 changes: 3 additions & 3 deletions march_hardware/test/mocks/mock_motor_controller.h
Original file line number Diff line number Diff line change
@@ -18,7 +18,7 @@ class MockMotorController : public march::MotorController
MOCK_METHOD0(prepareActuation, void());
MOCK_CONST_METHOD0(getActuationMode, march::ActuationMode());

MOCK_CONST_METHOD0(getSlaveIndex, uint16_t());
MOCK_CONST_METHOD0(getSlaveIndex, int());
MOCK_CONST_METHOD0(getIncrementalMorePrecise, bool());
MOCK_METHOD0(getStates, march::MotorControllerStates&());

@@ -30,10 +30,10 @@ class MockMotorController : public march::MotorController
MOCK_METHOD0(getMotorControllerVoltage, float());
MOCK_METHOD0(getMotorVoltage, float());
MOCK_METHOD0(getMotorCurrent, float());
MOCK_METHOD0(getTorque, int16_t());
MOCK_METHOD0(getTorque, double());

MOCK_METHOD1(actuateRad, void(double));
MOCK_METHOD1(actuateTorque, void(int16_t));
MOCK_METHOD1(actuateTorque, void(double));

MOCK_METHOD1(initialize, bool(int cycle_time));
MOCK_METHOD0(reset, void());
Original file line number Diff line number Diff line change
@@ -90,8 +90,8 @@ class MarchHardwareInterface : public hardware_interface::RobotHW
static void getSoftJointLimitsError(const std::string& name, const urdf::JointConstSharedPtr& urdf_joint,
joint_limits_interface::SoftJointLimits& error_soft_limits);

/* Limit of the change in effort command over one cycle, can be overridden by safety controller */
static constexpr double MAX_EFFORT_CHANGE = 5000;
/* Limit of the change in effort (ampere) command over one cycle, can be overridden by safety controller */
static constexpr double MAX_EFFORT_CHANGE = 6;

/* March hardware */
std::unique_ptr<march::MarchRobot> march_robot_;
4 changes: 2 additions & 2 deletions march_hardware_interface/src/march_hardware_interface.cpp
Original file line number Diff line number Diff line change
@@ -231,7 +231,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat
for (size_t i = 0; i < num_joints_; i++)
{
// Enlarge joint_effort_command because ROS control limits the pid values to a certain maximum
joint_effort_command_[i] = joint_effort_command_[i] * 1000.0;
joint_effort_command_[i] = joint_effort_command_[i];
if (std::abs(joint_last_effort_command_[i] - joint_effort_command_[i]) > MAX_EFFORT_CHANGE)
{
joint_effort_command_[i] =
@@ -277,7 +277,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat
}
else if (joint.getActuationMode() == march::ActuationMode::torque)
{
joint.actuateTorque(std::round(joint_effort_command_[i]));
joint.actuateTorque(joint_effort_command_[i]);
}
}
}