void Irp6otmM2J::updateHook() {
if (RTT::NewData == port_desired_motor_position_.read(motor_position_)) {
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
desired_motor_pos_initiated_ = true;
} else if ((!desired_motor_pos_initiated_)
&& (RTT::NewData == port_motor_position_.read(motor_position_))) {
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
}
}
void SarkofagJ2M::updateHook() {
if(port_joint_position_.read(joint_position_) == RTT::NewData) {
if(i2mp(&joint_position_(0), &motor_position_(0))) {
port_motor_position_.write(motor_position_);
}
}
}
void HwModel::updateHook() {
if (RTT::NewData == port_desired_input_.read(desired_input_)) {
// std::cout << "HwModel updateHook" << desired_input_(1) << std::endl;
// pytanie czy to nie przychodzi w inkrementach
for (int servo = 0; servo < number_of_servos_; servo++) {
// PWM input do implementacji
/*
if (!current_input_[servo]) { // pwm input
motor_position_(servo) = desired_input_(servo);
} else { // current input
*/
// prad jest w miliamperach
desired_torque_(servo) = desired_input_(servo) * torque_constant_[servo];
for (int iteration = 0; iteration < iteration_per_step_; iteration++) {
effective_torque_(servo) = desired_torque_(servo)
- motor_velocity_(servo) * viscous_friction_[servo];
motor_acceleration_(servo) = effective_torque_(servo) / inertia_[servo];
motor_velocity_(servo) += motor_acceleration_(servo) / m_factor_;
motor_position_(servo) += motor_velocity_(servo) / m_factor_;
}
//}
}
port_motor_position_.write(motor_position_);
}
}
bool HardwareInterfaceMW::startHook() {
try {
hi_->write_read_hardware(rwh_nsec_, 0);
servo_start_iter_ = 200;
for (int i = 0; i < number_of_drives_; i++) {
motor_position_(i) = static_cast<double>(hi_->get_position(i))
* ((2.0 * M_PI) / enc_res_[i]);
}
if (!hi_->robot_synchronized()) {
port_is_synchronised_.write(false);
RTT::log(RTT::Info) << "Robot not synchronized" << RTT::endlog();
synchro_start_iter_ = 500;
synchro_stop_iter_ = 1000;
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
synchro_drive_ = 0;
if (auto_synchronize_) {
RTT::log(RTT::Info) << "Auto synchronize" << RTT::endlog();
std::cout << getName() << " Auto synchronize" << std::endl;
state_ = PRE_SYNCHRONIZING;
} else {
RTT::log(RTT::Info) << "Manual synchronize" << RTT::endlog();
std::cout << getName() << " Manual synchronize" << std::endl;
state_ = NOT_SYNCHRONIZED;
}
} else {
port_is_synchronised_.write(true);
RTT::log(RTT::Info) << "Robot synchronized" << RTT::endlog();
for (int i = 0; i < number_of_drives_; i++) {
motor_position_command_(i) = motor_position_(i);
motor_increment_(i) = static_cast<double>(hi_->get_increment(i));
motor_current_(i) = static_cast<double>(hi_->get_current(i));
}
state_ = PRE_SERVOING;
}
} catch (const std::exception& e) {
RTT::log(RTT::Error) << e.what() << RTT::endlog();
return false;
}
for (int i = 0; i < number_of_drives_; i++) {
port_regulator_reset_list_[i]->write(static_cast<double>(true));
desired_position_[i] = motor_position_(i);
}
return true;
}
void HardwareInterfaceMW::updateHookInit() {
for (int i = 0; i < number_of_drives_; i++) {
if (RTT::NewData != computedReg_in_list_[i]->read(pwm_or_current_[i])) {
RTT::log(RTT::Error) << "NO PWM DATA" << RTT::endlog();
}
}
for (int i = 0; i < number_of_drives_; i++) {
if (current_mode_[i]) {
hi_->set_current(i, (double) pwm_or_current_[i]);
} else {
hi_->set_pwm(i, (double) pwm_or_current_[i]);
}
}
hi_->write_read_hardware(rwh_nsec_, timeouts_to_print_);
if (hi_->hardware_panic) {
port_is_hardware_panic_.write(true);
if (error_msg_hardware_panic_ == 0) {
std::cout << RED << std::endl << getName() << " [error] hardware panic"
<< RESET << std::endl << std::endl;
error_msg_hardware_panic_++;
}
} else {
port_is_hardware_panic_.write(false);
}
for (int i = 0; i < number_of_drives_; i++) {
motor_position_(i) = static_cast<double>(hi_->get_position(i))
* ((2.0 * M_PI) / enc_res_[i]);
}
bool emergency_stop;
if (port_emergency_stop_.read(emergency_stop) == RTT::NewData) {
std::cout << RED << std::endl << getName() << " emergency_stop" << RESET
<< std::endl << std::endl;
if (emergency_stop) {
hi_->set_hardware_panic();
}
}
for (int i = 0; i < number_of_drives_; i++) {
motor_current_(i) = static_cast<double>(hi_->get_current(i));
motor_increment_[i] = hi_->get_increment(i);
port_motor_current_list_[i]->write(static_cast<double>(motor_current_[i]));
double tmp_pos = motor_position_[i] * (enc_res_[i] / (2.0 * M_PI));
port_motor_position_list_[i]->write(static_cast<double>(tmp_pos));
/* port_motor_increment_list_[i]->write(
static_cast<double>(motor_increment_[i]));
*/
}
}
void HwModel::updateHook() {
for (int servo = 0; servo < number_of_servos_; servo++) {
port_desired_input_list_[servo]->read(desired_input_[servo]);
// prad jest w przeliczany do amperow
desired_torque_(servo) = desired_input_(servo) * torque_constant_[servo]
/ input_current_units_per_amper_[servo];
for (int iteration = 0; iteration < iteration_per_step_; iteration++) {
effective_torque_(servo) = desired_torque_(servo)
- motor_velocity_(servo) * viscous_friction_[servo];
motor_acceleration_(servo) = effective_torque_(servo) / inertia_[servo];
motor_velocity_(servo) += motor_acceleration_(servo) / m_factor_;
motor_position_(servo) += motor_velocity_(servo) / m_factor_;
}
inc_motor_position_[servo] = motor_position_[servo] * enc_res_[servo]
/ (2.0 * M_PI);
port_motor_position_list_[servo]->write(inc_motor_position_[servo]);
}
}
bool HwModel::configureHook() {
number_of_servos_ = torque_constant_.size();
if ((number_of_servos_ != inertia_.size())
|| (number_of_servos_ != viscous_friction_.size())
|| (number_of_servos_ != current_input_.size())) {
std::cout
<< std::endl
<< RED
<< "[error] hardware model "
<< getName()
<< "configuration failed: wrong properties vector length in launch file."
<< RESET << std::endl;
return false;
}
motor_position_.resize(number_of_servos_);
motor_velocity_.resize(number_of_servos_);
motor_acceleration_.resize(number_of_servos_);
desired_input_.resize(number_of_servos_);
desired_torque_.resize(number_of_servos_);
effective_torque_.resize(number_of_servos_);
port_motor_position_.setDataSample(motor_position_);
for (int i = 0; i < number_of_servos_; i++) {
motor_position_(i) = 0.0;
motor_velocity_(i) = 0.0;
motor_acceleration_(i) = 0.0;
desired_input_(i) = 0.0;
desired_torque_(i) = 0.0;
effective_torque_(i) = 0.0;
}
m_factor_ = step_per_second_ * iteration_per_step_;
return true;
}
void SarkofagM2J::updateHook() {
if (RTT::NewData == port_motor_position_.read(motor_position_)) {
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
}
}
void HardwareInterfaceMW::updateHookStateMachine() {
switch (state_) {
case NOT_SYNCHRONIZED: {
std_msgs::Bool do_synchro;
if (port_do_synchro_.read(do_synchro) == RTT::NewData) {
if (do_synchro.data) {
if (!test_mode_) {
state_ = PRE_SYNCHRONIZING;
}
std::cout << getName() << " NOT_SYNCHRONIZED" << std::endl;
}
}
}
break;
case PRE_SERVOING:
for (int i = 0; i < number_of_drives_; i++) {
motor_position_command_(i) = motor_position_(i);
port_motor_position_list_[i]->write(
static_cast<double>(motor_position_[i]));
}
if ((servo_start_iter_--) <= 0) {
state_ = SERVOING;
port_is_hardware_busy_.write(false);
std::cout << getName() << " Servoing started" << std::endl;
}
break;
case SERVOING: {
bool generator_active;
if (port_generator_active_.read(generator_active) == RTT::NewData) {
port_is_hardware_busy_.write(generator_active);
} else {
port_is_hardware_busy_.write(false);
}
for (int i = 0; i < number_of_drives_; i++) {
if (port_motor_position_command_list_[i]->read(
motor_position_command_[i]) == RTT::NewData) {
desired_position_[i] = motor_position_command_(i);
}
port_motor_position_list_[i]->write(
static_cast<double>(motor_position_[i]));
}
}
break;
case PRE_SYNCHRONIZING:
if ((synchro_start_iter_--) <= 0) {
state_ = SYNCHRONIZING;
std::cout << getName() << " Synchronization started" << std::endl;
}
break;
case SYNCHRONIZING:
switch (synchro_state_) {
case MOVE_TO_SYNCHRO_AREA:
if (synchro_needed_[synchro_drive_]) {
if (hi_->in_synchro_area(synchro_drive_)) {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_TO_SYNCHRO_AREA ended"
<< RTT::endlog();
synchro_state_ = STOP;
} else {
// ruszam powoli w stronę synchro area
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_TO_SYNCHRO_AREA"
<< RTT::endlog();
desired_position_[synchro_drive_] +=
synchro_step_coarse_[synchro_drive_];
}
} else {
hi_->set_parameter_now(synchro_drive_, NF_COMMAND_SetDrivesMisc,
NF_DrivesMisc_SetSynchronized);
hi_->reset_position(synchro_drive_);
if (++synchro_drive_ == number_of_drives_) {
synchro_state_ = SYNCHRO_END;
} else {
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
}
}
break;
case STOP:
hi_->start_synchro(synchro_drive_);
synchro_state_ = MOVE_FROM_SYNCHRO_AREA;
break;
case MOVE_FROM_SYNCHRO_AREA:
if (!hi_->in_synchro_area(synchro_drive_)) {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_FROM_SYNCHRO_AREA ended"
<< RTT::endlog();
synchro_state_ = WAIT_FOR_IMPULSE;
} else {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_FROM_SYNCHRO_AREA"
<< RTT::endlog();
desired_position_[synchro_drive_] +=
synchro_step_fine_[synchro_drive_];
}
break;
case WAIT_FOR_IMPULSE:
if (hi_->drive_synchronized(synchro_drive_)) {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] WAIT_FOR_IMPULSE ended"
<< RTT::endlog();
hi_->finish_synchro(synchro_drive_);
hi_->reset_position(synchro_drive_);
if (++synchro_drive_ == number_of_drives_) {
synchro_state_ = SYNCHRO_END;
} else {
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
}
} else {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] WAIT_FOR_IMPULSE" << RTT::endlog();
desired_position_[synchro_drive_] +=
synchro_step_fine_[synchro_drive_];
}
break;
case SYNCHRO_END:
if ((synchro_stop_iter_--) == 1) {
for (int i = 0; i < number_of_drives_; i++) {
motor_position_command_(i) = motor_position_(i);
desired_position_[i] = motor_position_(i);
}
port_is_synchronised_.write(true);
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] SYNCHRONIZING ended" << RTT::endlog();
std::cout << getName() << " synchro finished" << std::endl;
} else if (synchro_stop_iter_ <= 0) {
state_ = PRE_SERVOING;
}
break;
}
break;
}
for (int i = 0; i < number_of_drives_; i++) {
if (state_ != SERVOING) {
desired_position_out_list_[i]->write(
static_cast<double>(desired_position_[i]));
}
}
}
void Irp6otmM2J::updateHook() {
port_motor_position_.read(motor_position_);
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
}
bool HwModel::configureHook() {
number_of_servos_ = torque_constant_.size();
if ((number_of_servos_ != torque_constant_.size())
|| (number_of_servos_ != input_current_units_per_amper_.size())
|| (number_of_servos_ != inertia_.size())
|| (number_of_servos_ != viscous_friction_.size())
|| (number_of_servos_ != enc_res_.size())) {
std::cout
<< std::endl
<< RED
<< "[error] hardware model "
<< getName()
<< " configuration failed: wrong properties vector length in launch file."
<< RESET << std::endl;
return false;
}
motor_position_.resize(number_of_servos_);
motor_velocity_.resize(number_of_servos_);
motor_acceleration_.resize(number_of_servos_);
desired_input_.resize(number_of_servos_);
desired_torque_.resize(number_of_servos_);
effective_torque_.resize(number_of_servos_);
inc_motor_position_.resize(number_of_servos_);
// port_motor_position_.setDataSample(motor_position_);
port_motor_position_list_.resize(number_of_servos_);
port_desired_input_list_.resize(number_of_servos_);
desired_position_out_list_.resize(number_of_servos_);
port_motor_position_command_list_.resize(number_of_servos_);
port_motor_current_list_.resize(number_of_servos_);
port_regulator_reset_list_.resize(number_of_servos_);
for (int i = 0; i < number_of_servos_; i++) {
char port_motor_position_name[32];
snprintf(port_motor_position_name, sizeof(port_motor_position_name),
"MotorPosition_%s", port_labels_[i].c_str());
port_motor_position_list_[i] = new typeof(*port_motor_position_list_[i]);
this->ports()->addPort(port_motor_position_name,
*port_motor_position_list_[i]);
char port_desired_input_name[32];
snprintf(port_desired_input_name, sizeof(port_desired_input_name),
"computedReg_in_%s", port_labels_[i].c_str());
port_desired_input_list_[i] = new typeof(*port_desired_input_list_[i]);
this->ports()->addPort(port_desired_input_name,
*port_desired_input_list_[i]);
char DesiredPosition_out_port_name[32];
snprintf(DesiredPosition_out_port_name,
sizeof(DesiredPosition_out_port_name), "DesiredPosition_out_%s",
port_labels_[i].c_str());
desired_position_out_list_[i] = new typeof(*desired_position_out_list_[i]);
this->ports()->addPort(DesiredPosition_out_port_name,
*desired_position_out_list_[i]);
char MotorPositionCommand_port_name[32];
snprintf(MotorPositionCommand_port_name,
sizeof(MotorPositionCommand_port_name), "MotorPositionCommand_%s",
port_labels_[i].c_str());
port_motor_position_command_list_[i] =
new typeof(*port_motor_position_command_list_[i]);
this->ports()->addPort(MotorPositionCommand_port_name,
*port_motor_position_command_list_[i]);
char MotorCurrent_port_name[32];
snprintf(MotorCurrent_port_name, sizeof(MotorCurrent_port_name),
"MotorCurrent_%s", port_labels_[i].c_str());
port_motor_current_list_[i] = new typeof(*port_motor_current_list_[i]);
this->ports()->addPort(MotorCurrent_port_name,
*port_motor_current_list_[i]);
char port_regulator_reset_name[32];
snprintf(port_regulator_reset_name, sizeof(port_regulator_reset_name),
"RegulatorResetOutput_%s", port_labels_[i].c_str());
port_regulator_reset_list_[i] = new typeof(*port_regulator_reset_list_[i]);
this->ports()->addPort(port_regulator_reset_name,
*port_regulator_reset_list_[i]);
motor_position_(i) = 0.0;
motor_velocity_(i) = 0.0;
motor_acceleration_(i) = 0.0;
desired_input_(i) = 0.0;
desired_torque_(i) = 0.0;
effective_torque_(i) = 0.0;
}
m_factor_ = step_per_second_ * iteration_per_step_;
return true;
}
