Skip to content

Commit

Permalink
Merge pull request #79 from nasa-jpl/kwehage-act-param
Browse files Browse the repository at this point in the history 
Expose actuator parameters
  • Loading branch information
@kwehage
kwehage authored Mar 21, 2023
2 parents b3c0e2e + a2c6cef commit 7adbdad
Show file tree
Hide file tree
Showing 57 changed files with 1,046 additions and 1,021 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
cmake_minimum_required (VERSION 3.11)
project(fastcat
DESCRIPTION "C++ EtherCAT Device Command & Control Library"
VERSION 0.4.8
VERSION 0.11.5
LANGUAGES C CXX
)

Expand Down
6 changes: 1 addition & 5 deletions src/device_base.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,7 @@ void fastcat::DeviceBase::SetLoopPeriod(double loop_period)
loop_period_ = loop_period;
}


void fastcat::DeviceBase::SetTime(double time){
state_->time = time;
}
void fastcat::DeviceBase::SetTime(double time) { state_->time = time; }

bool fastcat::DeviceBase::Write(fastcat::DeviceCmd& /* cmd */)
{
Expand All @@ -47,4 +44,3 @@ void fastcat::DeviceBase::Reset()
}

fastcat::FaultType fastcat::DeviceBase::Process() { return NO_FAULT; }

8 changes: 4 additions & 4 deletions src/device_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,16 +39,16 @@ class DeviceBase
std::vector<Signal> signals_;

protected:
std::string name_; ///< unique device name
double loop_period_; ///< only some devices need
std::string name_; ///< unique device name
double loop_period_; ///< only some devices need

/// device-level fault, manager also has fault status flag
bool device_fault_active_ = false;
bool device_fault_active_ = false;

std::shared_ptr<DeviceState> state_; ///< Fastcat state data

/// for intra-device commands
std::shared_ptr<std::queue<DeviceCmd>> cmd_queue_;
std::shared_ptr<std::queue<DeviceCmd>> cmd_queue_;
};

} // namespace fastcat
Expand Down
1 change: 0 additions & 1 deletion src/fastcat_devices/conditional.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,6 @@ bool fastcat::Conditional::ConfigFromYaml(YAML::Node node)

bool fastcat::Conditional::Read()
{

// update input signal
if (!UpdateSignal(signals_[0])) {
ERROR("Could not extract signal");
Expand Down
37 changes: 19 additions & 18 deletions src/fastcat_devices/fts.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,9 @@ bool fastcat::Fts::ConfigFromYaml(YAML::Node node)

double dummy;
if (ParseOptVal(node, "max_force", dummy)) {
ERROR("fastcat no longer accept L2 norm, configure your yaml values per axis");
ERROR(
"fastcat no longer accept L2 norm, configure your yaml values per "
"axis");
return false;
}

Expand Down Expand Up @@ -129,20 +131,22 @@ bool fastcat::Fts::Read()
fastcat::FaultType fastcat::Fts::Process()
{
if (!device_fault_active_) {
if(enable_fts_guard_fault_){
if (max_force_[0] < fabs(state_->fts_state.raw_fx) || max_force_[1] < fabs(state_->fts_state.raw_fy) || max_force_[2] < fabs(state_->fts_state.raw_fz) ||
max_torque_[0] < fabs(state_->fts_state.raw_tx) || max_torque_[1] < fabs(state_->fts_state.raw_ty) || max_torque_[2] < fabs(state_->fts_state.raw_tz)) {
if (enable_fts_guard_fault_) {
if (max_force_[0] < fabs(state_->fts_state.raw_fx) ||
max_force_[1] < fabs(state_->fts_state.raw_fy) ||
max_force_[2] < fabs(state_->fts_state.raw_fz) ||
max_torque_[0] < fabs(state_->fts_state.raw_tx) ||
max_torque_[1] < fabs(state_->fts_state.raw_ty) ||
max_torque_[2] < fabs(state_->fts_state.raw_tz)) {
ERROR(
"Force or torque measured by device %s exceeded maximum allowable "
"magnitude. Force: [x]: %f / %f, [y]: %f / %f, [z]: %f / %f "
"Torque: [x]: %f / %f, [y]: %f / %f, [z]: %f / %f",
name_.c_str(),
state_->fts_state.raw_fx, max_force_[0],
state_->fts_state.raw_fy, max_force_[1],
state_->fts_state.raw_fz, max_force_[2],
state_->fts_state.raw_tx, max_torque_[0],
state_->fts_state.raw_ty, max_torque_[1],
state_->fts_state.raw_tz, max_torque_[2]);
name_.c_str(), state_->fts_state.raw_fx, max_force_[0],
state_->fts_state.raw_fy, max_force_[1], state_->fts_state.raw_fz,
max_force_[2], state_->fts_state.raw_tx, max_torque_[0],
state_->fts_state.raw_ty, max_torque_[1], state_->fts_state.raw_tz,
max_torque_[2]);
return ALL_DEVICE_FAULT;
}
}
Expand All @@ -153,23 +157,20 @@ fastcat::FaultType fastcat::Fts::Process()
bool fastcat::Fts::Write(DeviceCmd& cmd)
{
if (cmd.type == FTS_TARE_CMD) {

// Do not permit taring if there is a fault
if(device_fault_active_){
if (device_fault_active_) {
ERROR("Taring FTS is not permited with a active fault, reset first");
return false;
}else{
} else {
for (int ii = 0; ii < FC_FTS_N_DIMS; ii++) {
sig_offset_[ii] = -wrench_[ii];
}
return true;
}
}
else if (cmd.type == FTS_ENABLE_GUARD_FAULT_CMD) {
} else if (cmd.type == FTS_ENABLE_GUARD_FAULT_CMD) {
enable_fts_guard_fault_ = cmd.fts_enable_guard_fault_cmd.enable;
return true;
}
else{
} else {
WARNING("That command type is not supported!");
return false;
}
Expand Down
16 changes: 9 additions & 7 deletions src/fastcat_devices/fts.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,17 +46,19 @@ class Fts : public DeviceBase

protected:
std::vector<std::vector<double>>
calibration_; ///< Calibration matrix of doubles for wrench calculation.
calibration_; ///< Calibration matrix of doubles for wrench calculation.
double wrench_[FC_FTS_N_DIMS] = {
0}; ///< Array where wrench values are stored.
double sig_offset_[FC_FTS_N_DIMS] = {
0}; ///< Double array for storing tare offsets.
0}; ///< Double array for storing tare offsets.

bool enable_fts_guard_fault_ = true;
double max_force_[3] = {0, 0, 0}; ///< If x,y,z axis of force components exceeds
///< these values, the entire fastcat system will fault.
double max_torque_[3] = {0, 0, 0}; ///< If x,y,z axis of torque components exceeds
///< these values, the entire fastcat system will fault.
bool enable_fts_guard_fault_ = true;
double max_force_[3] = {
0, 0, 0}; ///< If x,y,z axis of force components exceeds
///< these values, the entire fastcat system will fault.
double max_torque_[3] = {
0, 0, 0}; ///< If x,y,z axis of torque components exceeds
///< these values, the entire fastcat system will fault.
};

} // namespace fastcat
Expand Down
98 changes: 50 additions & 48 deletions src/fastcat_devices/function.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,18 +15,20 @@ fastcat::Function::Function()
state_->type = FUNCTION_STATE;
}

fastcat::FunctionType fastcat::FunctionTypeFromString(const std::string& function_type) {
if(function_type.compare("POLYNOMIAL") == 0) {
fastcat::FunctionType fastcat::FunctionTypeFromString(
const std::string& function_type)
{
if (function_type.compare("POLYNOMIAL") == 0) {
return POLYNOMIAL;
} else if(function_type.compare("SUMMATION") == 0) {
} else if (function_type.compare("SUMMATION") == 0) {
return SUMMATION;
} else if(function_type.compare("MULTIPLICATION") == 0) {
} else if (function_type.compare("MULTIPLICATION") == 0) {
return MULTIPLICATION;
} else if(function_type.compare("POWER") == 0) {
} else if (function_type.compare("POWER") == 0) {
return POWER;
} else if(function_type.compare("EXPONENTIAL") == 0) {
} else if (function_type.compare("EXPONENTIAL") == 0) {
return EXPONENTIAL;
} else if(function_type.compare("SIGMOID") == 0) {
} else if (function_type.compare("SIGMOID") == 0) {
return SIGMOID;
} else {
return BAD_FUNCTION_TYPE;
Expand All @@ -43,73 +45,76 @@ bool fastcat::Function::ConfigFromYaml(YAML::Node node)
if (!ParseVal(node, "function_type", function_type_string_)) {
return false;
}

function_type_ = fastcat::FunctionTypeFromString(function_type_string_);
if (!ConfigSignalsFromYaml(node, signals_, false)) {
return false;
}

switch(function_type_) {

case POLYNOMIAL: {

switch (function_type_) {
case POLYNOMIAL: {
if (!ParseVal(node, "order", polynomial_params_.order)) {
return false;
}

YAML::Node coeff_node;
if (!ParseList(node, "coefficients", coeff_node)) {
return false;
}

for (auto coeff = coeff_node.begin(); coeff != coeff_node.end(); ++coeff) {

for (auto coeff = coeff_node.begin(); coeff != coeff_node.end();
++coeff) {
polynomial_coefficients_.push_back((*coeff).as<double>());
}

if (polynomial_params_.order != static_cast<int>(polynomial_coefficients_.size()) - 1) {
ERROR("for a polynomial of %d-order, expecting %d coefficients. %lu found",
polynomial_params_.order, polynomial_params_.order + 1,
polynomial_coefficients_.size()
);

if (polynomial_params_.order !=
static_cast<int>(polynomial_coefficients_.size()) - 1) {
ERROR(
"for a polynomial of %d-order, expecting %d coefficients. %lu "
"found",
polynomial_params_.order, polynomial_params_.order + 1,
polynomial_coefficients_.size());
return false;
}
// print the coefficients
std::string coeff_str("y = ");
for (int i = 0; i <= polynomial_params_.order; ++i) {
char term[64];
if (i == polynomial_params_.order) {
snprintf(term, 64, "(%f)*x^%d ", polynomial_coefficients_[i], polynomial_params_.order - i);
snprintf(term, 64, "(%f)*x^%d ", polynomial_coefficients_[i],
polynomial_params_.order - i);
} else {
snprintf(term, 64, "(%f)*x^%d + ", polynomial_coefficients_[i], polynomial_params_.order - i);
snprintf(term, 64, "(%f)*x^%d + ", polynomial_coefficients_[i],
polynomial_params_.order - i);
}
coeff_str.append(term);
}
MSG("Function: %s %s", name_.c_str(), coeff_str.c_str());

if (signals_.size() != 1) {
ERROR("Expecting exactly one signal for Function");
return false;
}
}

break;
}

case SUMMATION:
case SUMMATION:
break;

case MULTIPLICATION: {
if (signals_.size() < 2) {
ERROR("Expecting at least two signals for Function");
return false;
}
}
break;
}

case POWER: {
if (signals_.size() != 1) {
ERROR("Expecting exactly one signal for Function");
return false;
}
}
if (!ParseVal(node, "exponent", power_params_.exponent)) {
return false;
}
Expand All @@ -120,12 +125,11 @@ bool fastcat::Function::ConfigFromYaml(YAML::Node node)
if (signals_.size() != 1) {
ERROR("Expecting exactly one signal for Function");
return false;
}
}
if (!ParseVal(node, "base", exponential_params_.base)) {
WARNING(
"No 'base' specified for Function with exponenent type; "
"using default value of 'e'"
);
"No 'base' specified for Function with exponenent type; "
"using default value of 'e'");
exponential_params_.base = exp(1.0);
}
break;
Expand All @@ -135,14 +139,13 @@ bool fastcat::Function::ConfigFromYaml(YAML::Node node)
if (signals_.size() != 1) {
ERROR("Expecting exactly one signal for Function");
return false;
}
}
break;
}

case BAD_FUNCTION_TYPE: {

ERROR("Could not determine function type: %s",
function_type_string_.c_str());
ERROR("Could not determine function type: %s",
function_type_string_.c_str());

return false;
}
Expand All @@ -153,45 +156,45 @@ bool fastcat::Function::ConfigFromYaml(YAML::Node node)

bool fastcat::Function::Read()
{
for(auto& signal : signals_) {
for (auto& signal : signals_) {
if (!UpdateSignal(signal)) {
ERROR("Could not extract signal");
return false;
}
}

switch(function_type_) {
switch (function_type_) {
case POLYNOMIAL: {
state_->function_state.output = 0.0;
for (int i = 0; i <= polynomial_params_.order; ++i) {
state_->function_state.output +=
pow(signals_[0].value, polynomial_params_.order - i) *
polynomial_coefficients_[i];
pow(signals_[0].value, polynomial_params_.order - i) *
polynomial_coefficients_[i];
}
break;
}
case SUMMATION: {
state_->function_state.output = 0.0;
for(auto& signal : signals_) {
for (auto& signal : signals_) {
state_->function_state.output += signal.value;
}
break;
}
case MULTIPLICATION: {
state_->function_state.output = 1.0;
for(auto& signal : signals_) {
for (auto& signal : signals_) {
state_->function_state.output *= signal.value;
}
break;
}
case POWER: {
state_->function_state.output =
pow(signals_[0].value, power_params_.exponent);
state_->function_state.output =
pow(signals_[0].value, power_params_.exponent);
break;
}
case EXPONENTIAL: {
state_->function_state.output =
pow(exponential_params_.base, signals_[0].value);
state_->function_state.output =
pow(exponential_params_.base, signals_[0].value);
break;
}
case SIGMOID: {
Expand All @@ -205,5 +208,4 @@ bool fastcat::Function::Read()
}

return true;

}
Loading

0 comments on commit 7adbdad

Please sign in to comment.