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 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 Irp6otmM2J::updateHook() { port_motor_position_.read(motor_position_); mp2i(&motor_position_(0), &joint_position_(0)); port_joint_position_.write(joint_position_); }
void JointLimitAvoidance::updateHook() { if (port_joint_position_.read(joint_position_) != RTT::NewData) { RTT::Logger::In in("JointLimitAvoidance::updateHook"); error(); RTT::log(RTT::Error) << "could not read port: " << port_joint_position_.getName() << RTT::endlog(); return; } if (joint_position_.size() != number_of_joints_) { RTT::Logger::In in("JointLimitAvoidance::updateHook"); error(); RTT::log(RTT::Error) << "wrong data size: " << joint_position_.size() << " on port: " << port_joint_position_.getName() << RTT::endlog(); return; } if (port_joint_velocity_.read(joint_velocity_) != RTT::NewData) { RTT::Logger::In in("JointLimitAvoidance::updateHook"); error(); RTT::log(RTT::Error) << "could not read port: " << port_joint_velocity_.getName() << RTT::endlog(); return; } if (joint_velocity_.size() != number_of_joints_) { RTT::Logger::In in("JointLimitAvoidance::updateHook"); error(); RTT::log(RTT::Error) << "wrong data size: " << joint_velocity_.size() << " on port: " << port_joint_velocity_.getName() << RTT::endlog(); return; } if (port_nullspace_torque_command_.read(nullspace_torque_command_) != RTT::NewData) { // this is acceptable nullspace_torque_command_.setZero(); } if (nullspace_torque_command_.size() != number_of_joints_) { RTT::Logger::In in("JointLimitAvoidance::updateHook"); error(); RTT::log(RTT::Error) << "wrong data size: " << nullspace_torque_command_.size() << " on port: " << port_nullspace_torque_command_.getName() << RTT::endlog(); return; } for (size_t i = 0; i < number_of_joints_; i++) { joint_torque_command_(i) = jointLimitTrq(upper_limit_[i], lower_limit_[i], limit_range_[i], max_trq_[i], joint_position_(i)); if (abs(joint_torque_command_(i)) > 0.001) { k_(i) = max_trq_[i]/limit_range_[i]; } else { k_(i) = 0.001; } } if (port_mass_matrix_.read(m_) != RTT::NewData) { RTT::Logger::In in("JointLimitAvoidance::updateHook"); error(); RTT::log(RTT::Error) << "could not read port: " << port_mass_matrix_.getName() << RTT::endlog(); return; } if (m_.cols() != number_of_joints_ || m_.rows() != number_of_joints_) { RTT::Logger::In in("JointLimitAvoidance::updateHook"); error(); RTT::log(RTT::Error) << "invalid mass matrix size: " << m_.cols() << " " << m_.rows() << RTT::endlog(); return; } tmpNN_ = k_.asDiagonal(); es_.compute(tmpNN_, m_); q_ = es_.eigenvectors().inverse(); k0_ = es_.eigenvalues(); tmpNN_ = k0_.cwiseSqrt().asDiagonal(); d_.noalias() = 2.0 * q_.adjoint() * 0.7 * tmpNN_ * q_; joint_torque_command_.noalias() -= d_ * joint_velocity_; joint_torque_command_.noalias() += nullspace_torque_command_; port_joint_torque_command_.write(joint_torque_command_); }