装配体Assembly¶

[Parameter]
Joints=7

[Joint0]
file = "RA605_0.stl"
type = Pedestal
color = "#66e299"
Position = "0,0,375"

[Joint1]
file = "RA605_1.stl"
type = RotatingPair
dh = "0,0,0,375"
Range = "-165,165"
color = "#e270d1"
Origin = "0,0,0"
RotateAaxis = "0, 0, 1"

[Joint2]
file = "RA605_2.stl"
type = RotatingPair
dh = "-90,30,-90,0"
Range = "-125,85"
color = "#9e68b5"
Origin = "0,30,0"
RotateAaxis = "1, 0, 0"

[Joint3]
file = "RA605_3.stl"
type = RotatingPair
dh = "0,340,0,0"
Range = "-55,185"
color = "#7a9e9b"
Origin = "0,30,340"
RotateAaxis = "1, 0, 0"

[Joint4]
file = "RA605_4.stl"
type = RotatingPair
dh = "-90,40,0,338"
Range = "-190,190"
color = "#6bcef2"
Origin = "0,0,380"
RotateAaxis = "0, 1, 0"

[Joint5]
file = "RA605_5.stl"
type = RotatingPair
dh = "-90,0,0,0"
Range = "-115,115"
color = "#aaf46d"
Origin = "0,368,380"
RotateAaxis = "1, 0, 0"

[Joint6]
file = "RA605_6.stl"
type = RotatingPair
dh = "-90,0,180,86.5"
Range = "-360,360"
color = "#8ecc9e"
Origin = "0,368,380"
RotateAaxis = "0, 1, 0"

void RobotData::Joint::Rote(const double &angle)
{
    if (type != RotatingPair) {
        return;
    }
    static const double M_Zero = 1e-6;
    if ((angle - rote_range1) < M_Zero) {
        cur_rote_angle = rote_range1;
    } else if ((angle - rote_range2) > M_Zero) {
        cur_rote_angle = rote_range2;
    } else {
        cur_rote_angle = angle;
    }
    assembly->SetOrientation(0, 0, 0);
    assembly->RotateWXYZ(cur_rote_angle,
                         rotate_axis.at(0),
                         rotate_axis.at(1),
                         rotate_axis.at(2));
}

vtkSmartPointer<vtkAssembly> RobotData::GetAssembly(const int &id)
{
    Q_ASSERT(id < joints_.size());
    return joints_[id].assembly;
}

vtkSmartPointer<vtkLODActor> RobotData::GetActor(const int &id)
{
    Q_ASSERT(id < joints_.size());
    return joints_[id].actor;
}

RobotData::RobotJointType RobotData::JointType2int(const QString &str)
{
    if (str == "Pedestal") {
        return Pedestal;
    } else if (str == "RotatingPair") {
        return RotatingPair;
    } else if (str == "SlidingPair") {
        return SlidingPair;
    } else {
        return Err;
    }
}

void RobotData::SetJointsAngle(QList<double> angle)
{
    Q_ASSERT(angle.size() == joints_.size());
    for (int i = 0; i < joints_.size(); i++) {
        joints_[i].Rote(angle.at(i));
    }
}

void RobotData::SetJointAngle(const int &id, const double &angle)
{
    Q_ASSERT(id < joints_.size());
    joints_[id].Rote(angle);
}

Eigen::MatrixXd RobotData::GetDhParam()
{
    return this->mdh_param_;
}

QList<QList<double>> RobotData::GetJointsRoteRange() const
{
    QList<QList<double>> result;
    foreach (auto joint, joints_) {
        result << QList<double> { joint.rote_range1, joint.rote_range2, joint.cur_rote_angle };
    }
    return result;
}

void RobotData::Initialize()
{
    QSettings *config = new QSettings(robot_file_, QSettings::IniFormat);
    const int joint_number = config->value("Parameter/Joints").toInt();
    const QString file_path = QFileInfo(robot_file_).path();
    mdh_param_ = Eigen::MatrixXd::Zero(joint_number, 4);
    for (int i = 0; i < joint_number; i++) {
        const QString key = QString("Joint%1").arg(i);
        const RobotJointType type = JointType2int(config->value(key + "/type").toString());
        const QColor color = QColor(config->value(key + "/color").toString());
        const QString file_name = file_path + "/" + config->value(key + "/file").toString();
        Joint joint;
        joint.type = type;
        this->LoadStl(file_name, joint);
        this->CreateActor(joint, color);
        this->CreateAssembly(joint);
        QStringList strs;
        if (type == Pedestal) {
            strs = config->value(key + "/Position").toString().split(",");
            joint.assembly->SetPosition(strs.at(0).toDouble(), strs.at(1).toDouble(), strs.at(2).toDouble());
            mdh_param_.row(i) = Eigen::MatrixXd::Zero(1, 4);
        } else if (type == RotatingPair) {
            strs = config->value(key + "/Origin").toString().split(",");
            joint.assembly->SetPosition(0, 0, 0);
            joint.assembly->SetOrigin(strs.at(0).toDouble(), strs.at(1).toDouble(), strs.at(2).toDouble());
            strs = config->value(key + "/RotateAaxis").toString().split(",");
            joint.rotate_axis = QList<double> { strs.at(0).toDouble(), strs.at(1).toDouble(), strs.at(2).toDouble() };
            strs = config->value(key + "/Range").toString().split(",");
            joint.rote_range1 = strs.at(0).toDouble();
            joint.rote_range2 = strs.at(1).toDouble();
            strs = config->value(key + "/dh").toString().split(",");
            Eigen::MatrixXd dh(1, 4);
            dh << strs.at(0).toDouble() * M_PI / 180.0,
              strs.at(1).toDouble(), strs.at(2).toDouble(),
              strs.at(3).toDouble() * M_PI / 180.0;
            mdh_param_.row(i) = dh;
            joint.cur_rote_angle = strs.at(0).toDouble();
        }
        joints_ << joint;
    }
}

void RobotData::LoadStl(const QString &file, RobotData::Joint &joint)
{

    vtkNew<vtkSTLReader> reader;
    reader->SetFileName(file.toLocal8Bit().data());
    reader->Update();

    vtkNew<vtkPolyData> vtp;
    vtp->DeepCopy(reader->GetOutput());
    joint.vtp = vtp;
}

void RobotData::CreateActor(RobotData::Joint &joint, const QColor &diffuse_color)
{
    vtkNew<vtkLODActor> actor;
    actor->GetProperty()->SetDiffuseColor(
      diffuse_color.redF(), diffuse_color.greenF(), diffuse_color.blueF());
    actor->GetProperty()->SetDiffuse(.8);
    actor->GetProperty()->SetSpecular(.5);
    actor->GetProperty()->SetSpecularColor(1, 1, 1);
    actor->GetProperty()->SetSpecularPower(30);

    vtkNew<vtkPolyDataMapper> mapper;
    mapper->SetInputData(joint.vtp);
    actor->SetMapper(mapper);

    joint.actor = actor;
}

void RobotData::CreateAssembly(RobotData::Joint &joint)
{
    vtkNew<vtkAssembly> assembly;
    joint.assembly = assembly;
    assembly->AddPart(joint.actor);

    if (joints_.size() >= 1) {
        joints_[joints_.size() - 1].assembly->AddPart(assembly);
    }
}

Manipulator::Manipulator()
{
    Eigen::MatrixXd q_init = Eigen::MatrixXd::Zero(6, 1); //关节角初始化为[0, 0, 0, 0, 0, 0]
    setJointAngle(q_init);

    Eigen::MatrixXd dh_param(6, 4); //机械臂的DH参数
    dh_param << 0, 375, 0, 0,
      -M_PI / 2, 0, 30, -M_PI / 2,
      0, 0, 340, 0,
      0, 338, 40, -M_PI / 2,
      0, 0, 0, M_PI / 2,
      M_PI, 86.5, 0, -M_PI / 2;
    setDhParam(dh_param); //初始化机械臂的DH参数

    Manipulator::joint_num = 6;
    Manipulator::arm_radius = 0.08; //机械臂连杆的半径(粗细)，暂时都按最粗处80mm计算

    Manipulator::max_ang = 3 * M_PI / 4 * Eigen::MatrixXd::Ones(Manipulator::joint_num, 1); //最大关节角135°
    Manipulator::min_ang = -3 * M_PI / 4 * Eigen::MatrixXd::Ones(Manipulator::joint_num, 1); //最小关节角-135°
}

// 正运动学求解
// 输入六个关节角的大小，输入的是第2、3、5、6个关节处在笛卡尔空间的三维坐标，3×4矩阵。
Eigen::MatrixXd Manipulator::fkine(Eigen::MatrixXd joint_angle)
{ //正运动学
    Eigen::MatrixXd joint_position(3, 6);
    Eigen::MatrixXd dh = Manipulator::getDhParam();
    Eigen::Matrix4Xd T = Eigen::Matrix4Xd::Identity(4, 4);

    for (int i = 0; i < joint_angle.rows(); i++) {
        double q = dh(i, 0); //关节角
        double d = dh(i, 1); //连杆偏距
        double a = dh(i, 2); //连杆长度
        double alpha = dh(i, 3); //扭转角

        Eigen::Matrix4Xd T_temp(4, 4);

        T_temp << cos(q), -sin(q) * cos(alpha), sin(q) * sin(alpha), a * cos(q),
          sin(q), cos(q) * cos(alpha), -cos(q) * sin(alpha), a * sin(q),
          0, sin(alpha), cos(alpha), d,
          0, 0, 0, 1;

        T = T * T_temp;

        joint_position.col(i) = T.block(0, 3, 3, 1);
    }

    return joint_position;
}