LibXR::Kinematic::EndPoint< Scalar > Class Template Reference

机器人末端点（EndPoint）类，继承自 Object。 EndPoint class representing the end effector of a robotic system, inheriting from Object. More...

#include <kinematic.hpp>

Inheritance diagram for LibXR::Kinematic::EndPoint< Scalar >:

Collaboration diagram for LibXR::Kinematic::EndPoint< Scalar >:

Public Member Functions
	EndPoint (Inertia< Scalar > &inertia)
	构造 EndPoint 末端点对象。 Constructs an EndPoint object.
void	SetTargetQuaternion (const Quaternion< Scalar > &quat)
	设置目标四元数方向。 Sets the target quaternion orientation.
void	SetTargetPosition (const Position< Scalar > &pos)
	设置目标位置。 Sets the target position.
void	SetErrorWeight (const Eigen::Matrix< Scalar, 6, 1 > &weight)
	设置误差权重矩阵。 Sets the error weight matrix.
void	SetMaxAngularVelocity (Scalar velocity)
	设置最大角速度。 Sets the maximum angular velocity.
Eigen::Matrix< Scalar, 3, 1 >	GetPositionError ()
	获取末端点位置误差。 Gets the position error of the end effector.
Eigen::Quaternion< Scalar >	GetQuaternionError ()
	获取末端点方向误差。 Gets the orientation error of the end effector.
void	SetMaxLineVelocity (Scalar velocity)
	设置最大线速度。 Sets the maximum linear velocity.
Eigen::Matrix< Scalar, 6, 1 >	CalcBackward (Scalar dt, int max_step=10, Scalar max_err=1e-3, Scalar step_size=1.0)
	计算逆运动学（IK），调整关节角度以达到目标位置和方向。 Computes inverse kinematics (IK) to adjust joint angles for reaching the target position and orientation.
Public Member Functions inherited from LibXR::Kinematic::Object< Scalar >
	Object (Inertia< Scalar > &inertia)
	使用惯性参数构造 Object 对象。 Constructs an Object using inertia parameters.
void	SetPosition (const Position< Scalar > &pos)
	设置物体的位置信息。 Sets the position of the object.
void	SetQuaternion (const Quaternion< Scalar > &quat)
	设置物体的旋转信息（四元数表示）。 Sets the rotation of the object using a quaternion.
virtual void	CalcBackward ()

Private Attributes
Eigen::Matrix< Scalar, 6, Eigen::Dynamic > *	jacobian_matrix_
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > *	delta_theta_
	关节角度调整量。 Joint angle adjustments.
Eigen::Matrix< Scalar, 6, 1 >	err_weight_
	误差权重矩阵。 Error weight matrix.
int	joint_num_ = 0
Quaternion< Scalar >	target_quat_
	目标四元数方向。 Target quaternion orientation.
Position< Scalar >	target_pos_
	目标位置。 Target position.
Scalar	max_angular_velocity_
Scalar	max_line_velocity_ = -1.0

Additional Inherited Members
Public Types inherited from LibXR::Kinematic::Object< Scalar >
typedef List::Node< Joint< Scalar > * >	Link
	关节链接类型。 Type for linking joints.
Data Fields inherited from LibXR::Kinematic::Object< Scalar >
List	joints
	物体的关节列表。 List of joints associated with the object.
Joint< Scalar > *	parent = nullptr
	指向父关节的指针。 Pointer to the parent joint.
Param	param_
	物体参数。 Object parameters.
Runtime	runtime_
	物体运行时状态。 Object runtime data.

Detailed Description

template<typename Scalar>
class LibXR::Kinematic::EndPoint< Scalar >

机器人末端点（EndPoint）类，继承自 Object。 EndPoint class representing the end effector of a robotic system, inheriting from Object.

该类用于处理机器人末端的目标位置与方向， 并计算逆运动学（IK）以调整关节角度，使末端达到期望位置。 This class handles the target position and orientation of the robot's end effector and computes inverse kinematics (IK) to adjust joint angles to reach the desired position.

Template Parameters

Scalar

角度和位置的存储类型。 The storage type for angles and positions.

Definition at line 265 of file kinematic.hpp.

Constructor & Destructor Documentation

EndPoint()

template<typename Scalar >

LibXR::Kinematic::EndPoint< Scalar >::EndPoint ( Inertia< Scalar > &  inertia )

inline

构造 EndPoint 末端点对象。 Constructs an EndPoint object.

Parameters

inertia

物体的惯性参数。 The inertia parameters of the object.

Definition at line 293 of file kinematic.hpp.

: Object<Scalar>(inertia) {}

Member Function Documentation

CalcBackward()

template<typename Scalar >

Eigen::Matrix< Scalar, 6, 1 > LibXR::Kinematic::EndPoint< Scalar >::CalcBackward ( Scalar  dt, int  max_step = 10, Scalar  max_err = 1e-3, Scalar  step_size = 1.0  )

inline

计算逆运动学（IK），调整关节角度以达到目标位置和方向。 Computes inverse kinematics (IK) to adjust joint angles for reaching the target position and orientation.

该函数采用雅可比矩阵求解最优关节角度调整量，并进行迭代优化。 This function uses the Jacobian matrix to compute optimal joint angle adjustments and performs iterative optimization.

Parameters

dt	时间步长。 Time step.
max_step	最大迭代步数。 Maximum number of iterations.
max_err	允许的最大误差。 Maximum allowable error.
step_size	逆运动学步长。 Step size for inverse kinematics.

Returns

计算后的误差向量。 The computed error vector.

Definition at line 391 of file kinematic.hpp.

  {
    Eigen::Matrix<Scalar, 6, 1> error;
 
    /* Initialize */
    if (jacobian_matrix_ == nullptr)
    {
      Object<Scalar> *tmp = this;
      for (int i = 0;; i++)
      {
        if (tmp->parent == nullptr)
        {
          joint_num_ = i;
          break;
        }
        tmp = tmp->parent->parent;
      }
 
      jacobian_matrix_ = new Eigen::Matrix<Scalar, 6, Eigen::Dynamic>(6, joint_num_);
 
      delta_theta_ = new Eigen::Matrix<Scalar, Eigen::Dynamic, 1>(joint_num_);
    }
 
    /* Apply Limition */
    Position<Scalar> target_pos = target_pos_;
    Quaternion<Scalar> target_quat = target_quat_;
    if (max_line_velocity_ > 0 && max_angular_velocity_ > 0)
    {
      Scalar max_pos_delta = max_angular_velocity_ * dt;
      Scalar max_angle_delta = max_line_velocity_ * dt;
 
      Position<Scalar> pos_err =
          Position<Scalar>(target_pos_ - this->runtime_.target.translation);
      Eigen::AngleAxis<Scalar> angle_err(
          Quaternion<Scalar>(target_quat_ / this->runtime_.target.rotation));
 
      Scalar pos_err_norm = pos_err.norm();
 
      if (pos_err_norm > max_pos_delta)
      {
        pos_err = (max_pos_delta / pos_err_norm) * pos_err;
        target_pos = this->runtime_.target.translation + pos_err;
      }
      else
      {
        target_pos = target_pos_;
      }
 
      if (angle_err.angle() > max_angle_delta)
      {
        angle_err.angle() = max_angle_delta;
        target_quat = this->runtime_.target.rotation * angle_err;
      }
      else
      {
        target_quat = target_quat_;
      }
    }
 
    for (int step = 0; step < max_step; ++step)
    {
      /* Calculate Error */
      error.template head<3>() = target_pos - this->runtime_.target.translation;
      error.template tail<3>() =
          (Eigen::Quaternion<Scalar>(this->runtime_.target.rotation).conjugate() *
           target_quat)
              .vec();
 
      error = err_weight_.array() * error.array();
 
      auto err_norm = error.norm();
 
      if (err_norm < max_err)
      {
        break;
      }
 
      /* Calculate Jacobian */
      do
      {
        Joint<Scalar> *joint = this->parent;
        for (int joint_index = 0; joint_index < joint_num_; joint_index++)
        {
          /* J = [translation[3], rotation[3]] */
          Eigen::Matrix<Scalar, 6, 1> d_transform;
          d_transform.template head<3>() = joint->runtime_.target_axis.cross(
              this->runtime_.target.translation - joint->runtime_.target.translation);
 
          d_transform.template tail<3>() = joint->runtime_.target_axis;
 
          jacobian_matrix_->col(joint_index) = d_transform;
 
          joint = joint->parent->parent;
        }
      } while (0);
 
      /* Calculate delta_theta: delta_theta = J^+ * error */
      *delta_theta_ =
          jacobian_matrix_->completeOrthogonalDecomposition().pseudoInverse() * error *
          step_size / std::sqrt(err_norm);
 
      /* Update Joint Angle */
      do
      {
        Joint<Scalar> *joint = this->parent;
 
        for (int joint_index = 0; joint_index < joint_num_; joint_index++)
        {
          Eigen::AngleAxis<Scalar> target_angle_axis_delta((*delta_theta_)(joint_index),
                                                           joint->runtime_.target_axis);
 
          target_angle_axis_delta =
              Quaternion<Scalar>(Eigen::Quaternion<Scalar>(target_angle_axis_delta)) /
              joint->runtime_.target.rotation;
 
          joint->SetTarget(joint->runtime_.target_angle.angle() +
                           (*delta_theta_)(joint_index)*joint->param_.ik_mult);
 
          joint = joint->parent->parent;
        }
      } while (0);
 
      /* Recalculate Forward Kinematics */
      do
      {
        Joint<Scalar> *joint = this->parent;
 
        for (int joint_index = 0; joint_index < joint_num_; joint_index++)
        {
          if (joint_index == joint_num_ - 1)
          {
            auto start_point = reinterpret_cast<StartPoint<Scalar> *>(joint->parent);
            start_point->CalcTargetForward();
            break;
          }
 
          joint = joint->parent->parent;
        }
      } while (0);
    }
 
    return error;
  }

GetPositionError()

template<typename Scalar >

Eigen::Matrix< Scalar, 3, 1 > LibXR::Kinematic::EndPoint< Scalar >::GetPositionError ( )

inline

获取末端点位置误差。 Gets the position error of the end effector.

该误差表示当前状态与目标位置之间的差值。 This error represents the difference between the current state and the target position.

Returns

位置误差向量。 The position error vector.

Definition at line 345 of file kinematic.hpp.

  {
    return target_pos_ - Object<Scalar>::runtime_.target.translation;
  }

GetQuaternionError()

template<typename Scalar >

Eigen::Quaternion< Scalar > LibXR::Kinematic::EndPoint< Scalar >::GetQuaternionError ( )

inline

获取末端点方向误差。 Gets the orientation error of the end effector.

计算当前方向和目标方向的四元数误差。 Computes the quaternion error between the current and target orientations.

Returns

方向误差的四元数。 The quaternion representing the orientation error.

Definition at line 359 of file kinematic.hpp.

  {
    return Object<Scalar>::runtime_.target.rotation / target_quat_;
  }

SetErrorWeight()

template<typename Scalar >

void LibXR::Kinematic::EndPoint< Scalar >::SetErrorWeight ( const Eigen::Matrix< Scalar, 6, 1 > &  weight )

inline

设置误差权重矩阵。 Sets the error weight matrix.

该矩阵用于调整不同方向上的误差贡献权重。 This matrix is used to adjust the contribution weights of errors in different directions.

Parameters

weight

误差权重矩阵。 The error weight matrix.

Definition at line 321 of file kinematic.hpp.

{ err_weight_ = weight; }

SetMaxAngularVelocity()

template<typename Scalar >

void LibXR::Kinematic::EndPoint< Scalar >::SetMaxAngularVelocity ( Scalar  velocity )

inline

设置最大角速度。 Sets the maximum angular velocity.

该函数用于限制末端点旋转变化的速率。 This function limits the rate of rotational changes at the end effector.

Parameters

velocity

角速度上限（负值表示无限制）。 The maximum angular velocity (negative value means no limit).

Definition at line 333 of file kinematic.hpp.

{ max_angular_velocity_ = velocity; }

SetMaxLineVelocity()

template<typename Scalar >

void LibXR::Kinematic::EndPoint< Scalar >::SetMaxLineVelocity ( Scalar  velocity )

inline

设置最大线速度。 Sets the maximum linear velocity.

该函数用于限制末端点线性移动的速率。 This function limits the rate of linear movement at the end effector.

Parameters

velocity

线速度上限（负值表示无限制）。 The maximum linear velocity (negative value means no limit).

Definition at line 374 of file kinematic.hpp.

{ max_line_velocity_ = velocity; }

SetTargetPosition()

template<typename Scalar >

void LibXR::Kinematic::EndPoint< Scalar >::SetTargetPosition ( const Position< Scalar > &  pos )

inline

设置目标位置。 Sets the target position.

Parameters

pos	目标位置。 The target position.

Definition at line 309 of file kinematic.hpp.

{ target_pos_ = pos; }

SetTargetQuaternion()

template<typename Scalar >

void LibXR::Kinematic::EndPoint< Scalar >::SetTargetQuaternion ( const Quaternion< Scalar > &  quat )

inline

设置目标四元数方向。 Sets the target quaternion orientation.

Parameters

quat	目标四元数。 The target quaternion.

Definition at line 301 of file kinematic.hpp.

{ target_quat_ = quat; }

Field Documentation

delta_theta_

template<typename Scalar >

Eigen::Matrix<Scalar, Eigen::Dynamic, 1>* LibXR::Kinematic::EndPoint< Scalar >::delta_theta_

private

Initial value:

=
      nullptr

关节角度调整量。 Joint angle adjustments.

Definition at line 271 of file kinematic.hpp.

err_weight_

template<typename Scalar >

Eigen::Matrix<Scalar, 6, 1> LibXR::Kinematic::EndPoint< Scalar >::err_weight_

private

Initial value:

=
      Eigen::Matrix<Scalar, 6, 1>::Constant(1)

误差权重矩阵。 Error weight matrix.

Definition at line 273 of file kinematic.hpp.

jacobian_matrix_

template<typename Scalar >

Eigen::Matrix<Scalar, 6, Eigen::Dynamic>* LibXR::Kinematic::EndPoint< Scalar >::jacobian_matrix_

private

Initial value:

=
      nullptr

雅可比矩阵，用于逆运动学计算。 Jacobian matrix for inverse kinematics calculations.

Definition at line 268 of file kinematic.hpp.

joint_num_

template<typename Scalar >

int LibXR::Kinematic::EndPoint< Scalar >::joint_num_ = 0

private

机器人末端至基座的关节数量。 Number of joints from the end effector to the base.

Definition at line 275 of file kinematic.hpp.

max_angular_velocity_

template<typename Scalar >

Scalar LibXR::Kinematic::EndPoint< Scalar >::max_angular_velocity_

private

Initial value:

=
      -1.0

最大角速度（若小于 0 则无约束）。 Maximum angular velocity (negative value means no limit).

Definition at line 280 of file kinematic.hpp.

max_line_velocity_

template<typename Scalar >

Scalar LibXR::Kinematic::EndPoint< Scalar >::max_line_velocity_ = -1.0

private

最大线速度（若小于 0 则无约束）。 Maximum linear velocity (negative value means no limit).

Definition at line 283 of file kinematic.hpp.

target_pos_

template<typename Scalar >

Position<Scalar> LibXR::Kinematic::EndPoint< Scalar >::target_pos_

private

目标位置。 Target position.

Definition at line 279 of file kinematic.hpp.

target_quat_

template<typename Scalar >

Quaternion<Scalar> LibXR::Kinematic::EndPoint< Scalar >::target_quat_

private

目标四元数方向。 Target quaternion orientation.

Definition at line 278 of file kinematic.hpp.

---

The documentation for this class was generated from the following file:

• src/utils/kinematic.hpp