ROS机械臂正逆运动学

这里做一个六轴机械臂用于正逆运动学实验。

这里其实一共只有3轴，只有3轴位置没有姿态。所以urdf文件里我在末端做了3个虚拟关节，以便将kdl的frame能够填满，使得齐次坐标变换是规则的。

1.urdf建模

<?xml version="1.0" ?>

<!--           -->
<robot name="mbot">

    <!--   -->
    <link name="base_link" >
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                    <cylinder length="0.2" radius="0.1" />
            </geometry>
            <material name="yellow">
                <color rgba="1 0.4 0 1" />
            </material>
        </visual>
    </link>

        <!-- -->
    <joint name="joint1" type="continuous" >
        <origin xyz="0 0 0.15" rpy="0 0 0" />
        <parent link="base_link" />
        <child link="link1" />
        <axis xyz="0 0 1" />
    </joint>

    <!--  -->
    <link name="link1">
    <visual>
        <origin xyz="0 0 0" rpy="-1.57 0 0" />
        <geometry>
            <cylinder radius="0.05" length="0.05" />
        </geometry>
        <material name="white">
            <color rgba="1 1 1 0.9" />
        </material>
    </visual>
    </link>

    <joint name="joint2" type="continuous" >
        <origin xyz="0 0.075 0" rpy="-1.57 0 0" />
        <parent link="link1" />
        <child link="link2" />
        <axis xyz="0 0 1" />
    </joint>

    <link name="link2">
    <visual>
        <origin xyz="0 -0.15 0" rpy="1.57 0 0" />
        <geometry>
            <cylinder radius="0.05" length="0.3" />
        </geometry>
        <material name="white">
            <color rgba="1 1 1 0.9" />
        </material>
    </visual>
    </link>

     <joint name="joint3" type="continuous" >
        <origin xyz="0 -0.3  0" rpy="0 0 0" />
        <parent link="link2" />
        <child link="link3" />
        <axis xyz="0 0 1" />
    </joint>

    <link name="link3">
    <visual>
        <origin xyz="0 -0.15 0" rpy="-1.57 0 0" />
        <geometry>
            <cylinder radius="0.05" length="0.3" />
        </geometry>
        <material name="white">
            <color rgba="1 1 1 0.9" />
        </material>
    </visual>
    </link>

    <joint name="joint4" type="continuous" >
        <origin xyz="0 -0.3  0" rpy="1.57 0 0" />
        <parent link="link3" />
        <child link="link4" />
        <axis xyz="0 0 1" />
    </joint>

    <link name="link4">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <cylinder radius="0" length="0" />
        </geometry>
        <material name="white">
            <color rgba="1 1 1 0.9" />
        </material>
    </visual>
    </link>

     <joint name="joint5" type="continuous" >
        <origin xyz="0 0 0" rpy="-1.57 0 0" />
        <parent link="link4" />
        <child link="link5" />
        <axis xyz="0 0 1" />
    </joint>

    <link name="link5">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <cylinder radius="0" length="0" />
        </geometry>
        <material name="white">
            <color rgba="1 1 1 0.9" />
        </material>
    </visual>
    </link>

    <joint name="joint6" type="continuous" >
        <origin xyz="0 0 0" rpy="0 1.57 0" />
        <parent link="link5" />
        <child link="link6" />
        <axis xyz="0 0 1" />
    </joint>

    <link name="link6">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <cylinder radius="0" length="0" />
        </geometry>
        <material name="white">
            <color rgba="1 1 1 0.9" />
        </material>
    </visual>
    </link>








</robot>

如下图：

2. 编写正逆运动学程序

我这里只定义了两个机械臂状态，让机械臂在两个状态之间来回切换，以达到运动的效果，由于是实验，就没有规划末端轨迹

程序如下：

#include <iostream>
#include <string>
#include <vector>

#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
#include "kdl_parser/kdl_parser.hpp"
#include "kdl/chainiksolverpos_nr_jl.hpp"
#include "trac_ik/trac_ik.hpp"
#include "urdf/model.h"
#define pi 3.141592653

int main(int argc, char* argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_tf_pub");
    ros::NodeHandle nh;
    ros::Publisher joint_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
    //定义tf坐标广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped ts;
    KDL::Vector v1(1,1,1);
    KDL::Tree my_tree;
    sensor_msgs::JointState joint_state;
    
    std::string robot_desc_string;
    nh.param("robot_description", robot_desc_string, std::string());
    
    if(!kdl_parser::treeFromString(robot_desc_string, my_tree))
    //if(!kdl_parser::treeFromFile("/home/zhitong/catkin_ws_serial/src/quadruped_9g-master/urdf/mbot_base.urdf", my_tree))
    {
        ROS_ERROR("Failed to construct kdl tree");
    }
    else
    {
        ROS_INFO("成功生成kdl树!");
    }
    std::vector<std::string> joint_name = {"joint1", "joint2", "joint3", "joint4","joint5","joint6"};
    std::vector<double> joint_pos = {0,0,0,0,0,0};
    
    std::string urdf_param = "/robot_description";
    double timeout = 0.005;
    double eps = 1e-5;
    std::string chain_start = "base_link"; 
    std::string chain_end = "link6"; 
    TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param, timeout, eps);
    KDL::Chain chain;
    KDL::JntArray ll, ul; //关节下限, 关节上限
    bool valid = tracik_solver.getKDLChain(chain);
    if(!valid)
    {
        ROS_ERROR("There was no valid KDL chain found");
    }
    valid = tracik_solver.getKDLLimits(ll, ul);
    if(!valid)
    {
        ROS_ERROR("There was no valid KDL joint limits found");
    }
    KDL::ChainFkSolverPos_recursive fk_solver(chain);
    ROS_INFO("关节数量: %d", chain.getNrOfJoints());
    KDL::JntArray nominal(6);
    ROS_INFO("the nominal size is:%d",nominal.data.size());
    for(size_t j = 0; j < 6; j ++)
    {
        nominal(j)=0.0;
        //nominal(j) = (ll(j) + ul(j))/2.0;
    }
    KDL::JntArray q(6); // 目标关节位置
    q(0)=0;
    q(1)=pi/3;
    q(2)=pi/3;
    q(3)=0;
    q(4)=0;
    q(5)=0;
    q(6)=0;
    //定义末端4x4齐次变换矩阵
    KDL::Frame end_effector_pose;
    //定义逆运动学解算结果存储数组
    KDL::JntArray result;
    ros::Rate r(1);

    bool flag=true;
    auto print_frame_lambda = [](KDL::Frame f)
    {
        double x, y, z, roll, pitch, yaw;
        x = f.p.x();
        y = f.p.y();
        z = f.p.z();
        f.M.GetRPY(roll, pitch, yaw);
        std::cout << "x:" << x << " y:" << y << " z:" << z << " roll:" << roll << " pitch:" << pitch << " yaw:" << yaw << std::endl;
    };

    //正运动学
    fk_solver.JntToCart(q,end_effector_pose);
    //逆运动学
    int rc = tracik_solver.CartToJnt(nominal, end_effector_pose, result);
    
    ROS_INFO("1:%f,2:%f,3:%f,4:%f,5:%f,6:%f",result(0),result(1),result(2),result(3),result(4),result(5));
    print_frame_lambda(end_effector_pose);
    
    
    while(ros::ok())
    {
    if(flag)
        {
            fk_solver.JntToCart(q, end_effector_pose);
            int rc = tracik_solver.CartToJnt(nominal, end_effector_pose, result);
            
            print_frame_lambda(end_effector_pose);
        }
        else
        {
            fk_solver.JntToCart(nominal, end_effector_pose); 
            int rc = tracik_solver.CartToJnt(q, end_effector_pose, result);
            print_frame_lambda(end_effector_pose);
        }
        flag = !flag;
        // 更新关节状态
        ROS_INFO("更新关节状态");
        joint_state.header.stamp = ros::Time::now();
	//ROS_INFO("%d",joint_state.header.stamp);
        joint_state.name.resize(6);
        joint_state.position.resize(6);

        for(size_t i = 0; i < 6; i ++)
        {
            joint_state.name[i] = joint_name[i];
            //joint_state.position[i] = result(i);
        }
    joint_state.position[0] = result(0);
    joint_state.position[1] = result(1);
    joint_state.position[2] = result(2);
    joint_state.position[3] = result(3);
    joint_state.position[4] = result(4);
    joint_state.position[5] = result(5);
    joint_pub.publish(joint_state);
    
    
    
    
    
    
    
    r.sleep();
    }
    return 0;

}

3.编写launch文件

<launch>

  <arg name="model" default="$(find quadruped_9g)/urdf/mbot_base.urdf"/>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find quadruped_9g)/rviz/urdf.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>

  <node name="test" pkg="quadruped_9g" type="test" output="screen"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

4.启动节点运行程序

 可以看到已经成功了！