Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3960] pkg/trunk
|
From: <rob...@us...> - 2008-09-04 23:06:26
|
Revision: 3960
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3960&view=rev
Author: rob_wheeler
Date: 2008-09-04 23:06:35 +0000 (Thu, 04 Sep 2008)
Log Message:
-----------
Move motor torque constant and pulses per revolution out of the transmission
and into the actuator. For now, the values are hard-coded. In the future
they will be read from the eeprom of the MCB.
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg
pkg/trunk/mechanism/mechanism_control/scripts/listen.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/simple_transmission.h
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-04 23:06:35 UTC (rev 3960)
@@ -151,7 +151,7 @@
current = current_buffer_;
for (unsigned int i = 0; i < hw_->actuators_.size(); ++i)
{
- hw_->actuators_[i]->state_.last_requested_current_ = hw_->actuators_[i]->command_.current_;
+ hw_->actuators_[i]->state_.last_requested_effort_ = hw_->actuators_[i]->command_.effort_;
slaves_[i]->truncateCurrent(hw_->actuators_[i]->command_);
slaves_[i]->convertCommand(hw_->actuators_[i]->command_, current);
current += slaves_[i]->command_size_ + slaves_[i]->status_size_;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-09-04 23:06:35 UTC (rev 3960)
@@ -32,6 +32,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
+#include <math.h>
+
#include <ethercat_hardware/wg05.h>
#include <dll/ethercat_dll.h>
@@ -155,13 +157,19 @@
max_current_ = config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_;
}
+static const double motor_torque_constant = -0.0603;
+static const int pulses_per_revolution = 1200;
+
void WG05::convertCommand(ActuatorCommand &command, unsigned char *buffer)
{
WG05Command c;
memset(&c, 0, sizeof(c));
- c.programmed_current_ = command.current_ / config_info_.nominal_current_scale_;
+ double current = command.effort_ / motor_torque_constant;
+ current = max(min(current, max_current_), -max_current_);
+
+ c.programmed_current_ = current / config_info_.nominal_current_scale_;
c.mode_ = command.enable_ ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
c.checksum_ = rotate_right_8(compute_checksum(&c, sizeof(c) - 1));
@@ -170,7 +178,7 @@
void WG05::truncateCurrent(ActuatorCommand &command)
{
- command.current_ = max(min(command.current_, max_current_), -max_current_);
+ //command.current_ = max(min(command.current_, max_current_), -max_current_);
}
void WG05::convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer)
@@ -186,16 +194,18 @@
state.timestamp_ = current_status.timestamp_ / 1e+6;
state.encoder_count_ = current_status.encoder_count_;
+ state.position_ = double(current_status.encoder_count_) / pulses_per_revolution * 2 * M_PI;
state.encoder_velocity_ = double(int(current_status.encoder_count_ - last_status.encoder_count_))
/ (current_status.timestamp_ - last_status.timestamp_) * 1e+6;
+ state.velocity_ = state.encoder_velocity_ / pulses_per_revolution * 2 * M_PI;
state.calibration_reading_ = current_status.calibration_reading_ & LIMIT_SENSOR_0_STATE;
state.last_calibration_high_transition_ = current_status.last_calibration_high_transition_;
state.last_calibration_low_transition_ = current_status.last_calibration_low_transition_;
state.is_enabled_ = current_status.mode_ != MODE_OFF;
state.run_stop_hit_ = (current_status.mode_ & MODE_UNDERVOLTAGE) != 0;
- state.last_commanded_current_ = current_status.programmed_current_ * config_info_.nominal_current_scale_;
- state.last_measured_current_ = current_status.measured_current_ * config_info_.nominal_current_scale_;
+ state.last_commanded_effort_ = current_status.programmed_current_ * config_info_.nominal_current_scale_ * motor_torque_constant;
+ state.last_measured_effort_ = current_status.measured_current_ * config_info_.nominal_current_scale_ * motor_torque_constant;
state.num_encoder_errors_ = current_status.num_encoder_errors_;
state.num_communication_errors_ = 0; // TODO: communication errors are no longer reported in the process data
Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-09-04 23:06:35 UTC (rev 3960)
@@ -46,23 +46,28 @@
last_calibration_low_transition_(0),
is_enabled_(0),
run_stop_hit_(0),
- last_requested_current_(0),
- last_commanded_current_(0),
- last_measured_current_(0),
+ last_requested_effort_(0),
+ last_commanded_effort_(0),
+ last_measured_effort_(0),
num_encoder_errors_(0)
{}
+ double timestamp_;
+
int encoder_count_;
- double timestamp_;
+ double position_;
double encoder_velocity_;
+ double velocity_;
+
bool calibration_reading_;
int last_calibration_high_transition_;
int last_calibration_low_transition_;
+
bool is_enabled_;
bool run_stop_hit_;
- double last_requested_current_;
- double last_commanded_current_;
- double last_measured_current_;
+ double last_requested_effort_;
+ double last_commanded_effort_;
+ double last_measured_effort_;
int motor_voltage_;
@@ -74,10 +79,10 @@
{
public:
ActuatorCommand() :
- enable_(0), current_(0)
+ enable_(0), effort_(0)
{}
bool enable_;
- double current_;
+ double effort_;
};
class Actuator
Modified: pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg
===================================================================
--- pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg 2008-09-04 23:06:35 UTC (rev 3960)
@@ -1,15 +1,17 @@
string name
int32 encoder_count
+float64 position
float64 timestamp
float64 encoder_velocity
+float64 velocity
byte calibration_reading
int32 last_calibration_high_transition
int32 last_calibration_low_transition
byte is_enabled
byte run_stop_hit
-float64 last_requested_current
-float64 last_commanded_current
-float64 last_measured_current
+float64 last_requested_effort
+float64 last_commanded_effort
+float64 last_measured_effort
int32 motor_voltage
int32 num_encoder_errors
Modified: pkg/trunk/mechanism/mechanism_control/scripts/listen.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-09-04 23:06:35 UTC (rev 3960)
@@ -19,16 +19,18 @@
for a in data.actuator_states:
print " actuator: %s" % a.name
print " encoder_count: %d" % a.encoder_count
+ print " position: %d" % a.position
print " timestamp: %.4f" % a.timestamp
print " encoder_velocity: %.4f" % a.encoder_velocity
+ print " velocity: %.4f" % a.velocity
print " calibration_reading: %d" % a.calibration_reading
print " last_calibration_high_transition: %d" % a.last_calibration_high_transition
print " last_calibration_low_transition: %d" % a.last_calibration_low_transition
print " is_enabled: %d" % a.is_enabled
print " run_stop_hit: %d" % a.run_stop_hit
- print " last_requested_current: %.4f" % a.last_requested_current
- print " last_commanded_current: %.4f" % a.last_commanded_current
- print " last_measured_current: %.4f" % a.last_measured_current
+ print " last_requested_effort: %.4f" % a.last_requested_effort
+ print " last_commanded_effort: %.4f" % a.last_commanded_effort
+ print " last_measured_effort: %.4f" % a.last_measured_effort
print " motor_voltage: %.4f" % a.motor_voltage
print " num_encoder_errors: %d" % a.num_encoder_errors
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-04 23:06:35 UTC (rev 3960)
@@ -248,16 +248,18 @@
ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
out->name = mc_->hw_->actuators_[i]->name_;
out->encoder_count = in->encoder_count_;
+ out->position = in->position_;
out->timestamp = in->timestamp_;
out->encoder_velocity = in->encoder_velocity_;
+ out->velocity = in->velocity_;
out->calibration_reading = in->calibration_reading_;
out->last_calibration_high_transition = in->last_calibration_high_transition_;
out->last_calibration_low_transition = in->last_calibration_low_transition_;
out->is_enabled = in->is_enabled_;
out->run_stop_hit = in->run_stop_hit_;
- out->last_requested_current = in->last_requested_current_;
- out->last_commanded_current = in->last_commanded_current_;
- out->last_measured_current = in->last_measured_current_;
+ out->last_requested_effort = in->last_requested_effort_;
+ out->last_commanded_effort = in->last_commanded_effort_;
+ out->last_measured_effort = in->last_measured_effort_;
out->motor_voltage = in->motor_voltage_;
out->num_encoder_errors = in->num_encoder_errors_;
}
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/simple_transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/simple_transmission.h 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/simple_transmission.h 2008-09-04 23:06:35 UTC (rev 3960)
@@ -53,8 +53,6 @@
bool initXml(TiXmlElement *config, Robot *robot);
double mechanical_reduction_;
- double motor_torque_constant_;
- double pulses_per_revolution_;
void propagatePosition(std::vector<Actuator*>&, std::vector<JointState*>&);
void propagatePositionBackwards(std::vector<JointState*>&, std::vector<Actuator*>&);
Modified: pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-09-04 23:06:35 UTC (rev 3960)
@@ -92,10 +92,9 @@
assert(js.size() == reductions_.size());
for (unsigned int i = 0; i < js.size(); ++i)
{
- double mr = reductions_[i];
- js[i]->position_ = as[0]->state_.encoder_count_ * 2 * M_PI / (mr * pulses_per_revolution_);
- js[i]->velocity_ = as[0]->state_.encoder_velocity_ * 2 * M_PI / (mr * pulses_per_revolution_);
- js[i]->applied_effort_ = as[0]->state_.last_measured_current_ * mr * motor_torque_constant_;
+ js[i]->position_ = as[0]->state_.position_ / reductions_[i];
+ js[i]->velocity_ = as[0]->state_.velocity_ / reductions_[i];
+ js[i]->applied_effort_ = as[0]->state_.last_measured_effort_ * reductions_[i];
}
}
@@ -105,19 +104,18 @@
assert(as.size() == 1);
assert(js.size() == reductions_.size());
- double mean_encoder = 0.0;
- double mean_encoder_v = 0.0;
- double mean_current = 0.0;
+ double mean_position = 0.0;
+ double mean_velocity = 0.0;
+ double mean_effort = 0.0;
for (unsigned int i = 0; i < js.size(); ++i)
{
- double mr = reductions_[i];
- mean_encoder += js[i]->position_ * mr * pulses_per_revolution_ / (2 * M_PI);
- mean_encoder_v += js[i]->velocity_ * mr * pulses_per_revolution_ / (2 * M_PI);
- mean_current += js[i]->applied_effort_ / (mr * motor_torque_constant_);
+ mean_position += js[i]->position_ * reductions_[i];
+ mean_velocity += js[i]->velocity_ * reductions_[i];
+ mean_effort += js[i]->applied_effort_ / (reductions_[i]);
}
- as[0]->state_.encoder_count_ = mean_encoder / js.size();
- as[0]->state_.encoder_velocity_ = mean_encoder_v / js.size();
- as[0]->state_.last_measured_current_ = mean_current / js.size();
+ as[0]->state_.position_ = mean_position / js.size();
+ as[0]->state_.velocity_ = mean_velocity / js.size();
+ as[0]->state_.last_measured_effort_ = mean_effort / js.size();
}
void GripperTransmission::propagateEffort(
@@ -130,9 +128,9 @@
for (unsigned int i = 0; i < js.size(); ++i)
{
if (fabs(js[i]->commanded_effort_ / (reductions_[i])) > fabs(strongest))
- strongest = js[i]->commanded_effort_ / (reductions_[i] * motor_torque_constant_);
+ strongest = js[i]->commanded_effort_ / reductions_[i];
}
- as[0]->command_.current_ = strongest;
+ as[0]->command_.effort_ = strongest;
}
void GripperTransmission::propagateEffortBackwards(
@@ -154,8 +152,7 @@
double pid_effort = pids_[i].updatePid(err, 0.001);
js[i]->commanded_effort_ =
- pid_effort / reductions_[i] +
- as[0]->command_.current_ * reductions_[i] * motor_torque_constant_;
+ pid_effort / reductions_[i] + as[0]->command_.effort_ * reductions_[i];
}
}
Modified: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-09-04 23:06:35 UTC (rev 3960)
@@ -62,9 +62,7 @@
}
actuator_names_.push_back(actuator_name);
- mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
- motor_torque_constant_ = atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
- pulses_per_revolution_ = atof(elt->FirstChildElement("pulsesPerRevolution")->GetText());
+ mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
return true;
}
@@ -73,9 +71,9 @@
{
assert(as.size() == 1);
assert(js.size() == 1);
- js[0]->position_ = ((double)as[0]->state_.encoder_count_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
- js[0]->velocity_ = ((double)as[0]->state_.encoder_velocity_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
- js[0]->applied_effort_ = as[0]->state_.last_measured_current_ * (motor_torque_constant_ * mechanical_reduction_);
+ js[0]->position_ = as[0]->state_.position_ / mechanical_reduction_;
+ js[0]->velocity_ = as[0]->state_.velocity_ / mechanical_reduction_;
+ js[0]->applied_effort_ = as[0]->state_.last_measured_effort_ * mechanical_reduction_;
}
void SimpleTransmission::propagatePositionBackwards(
@@ -83,9 +81,9 @@
{
assert(as.size() == 1);
assert(js.size() == 1);
- as[0]->state_.encoder_count_ = (int)(js[0]->position_ * pulses_per_revolution_ * mechanical_reduction_ / (2*M_PI));
- as[0]->state_.encoder_velocity_ = js[0]->velocity_ * pulses_per_revolution_ * mechanical_reduction_ / (2*M_PI);
- as[0]->state_.last_measured_current_ = js[0]->applied_effort_ / (motor_torque_constant_ * mechanical_reduction_);
+ as[0]->state_.position_ = js[0]->position_ * mechanical_reduction_;
+ as[0]->state_.velocity_ = js[0]->velocity_ * mechanical_reduction_;
+ as[0]->state_.last_measured_effort_ = js[0]->applied_effort_ / mechanical_reduction_;
}
void SimpleTransmission::propagateEffort(
@@ -93,7 +91,7 @@
{
assert(as.size() == 1);
assert(js.size() == 1);
- as[0]->command_.current_ = js[0]->commanded_effort_/(motor_torque_constant_ * mechanical_reduction_);
+ as[0]->command_.effort_ = js[0]->commanded_effort_ / mechanical_reduction_;
as[0]->command_.enable_ = true;
}
@@ -102,5 +100,5 @@
{
assert(as.size() == 1);
assert(js.size() == 1);
- js[0]->commanded_effort_ = as[0]->command_.current_ * motor_torque_constant_ * mechanical_reduction_;
+ js[0]->commanded_effort_ = as[0]->command_.effort_ * mechanical_reduction_;
}
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-09-04 22:32:36 UTC (rev 3959)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-09-04 23:06:35 UTC (rev 3960)
@@ -111,7 +111,7 @@
for (unsigned int i = 0; i < ec.hw_->actuators_.size(); ++i)
{
ec.hw_->actuators_[i]->command_.enable_ = false;
- ec.hw_->actuators_[i]->command_.current_ = 0;
+ ec.hw_->actuators_[i]->command_.effort_ = 0;
}
ec.update();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
