Exemplos de joint_position_ em C++ (Cpp)

Exemplo n.º 1

Arquivo: Irp6otmM2J.cpp Projeto: snyzcalm/irp6_robot

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_);
  }
}

Exemplo n.º 2

Arquivo: SarkofagJ2M.cpp Projeto: OlenkaKa/irp6_robot

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_);
	  }
	}
}

Exemplo n.º 3

Arquivo: SarkofagM2J.cpp Projeto: RCPRG-ros-pkg/sarkofag_robot

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_);
  }
}

Exemplo n.º 4

Arquivo: Irp6otmM2J.cpp Projeto: PatrykWasowski/irp6_robot

void Irp6otmM2J::updateHook() {
  port_motor_position_.read(motor_position_);
  mp2i(&motor_position_(0), &joint_position_(0));
  port_joint_position_.write(joint_position_);
}

Exemplo n.º 5

Arquivo: joint_limit_avoidance.cpp Projeto: RCPRG-ros-pkg/common_controllers

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_);
}