白泽四足机器人ROS+rviz仿真（一）正逆运动学（单腿）

导航在这里：白泽四足机器人导航贴

目录

​​源码：​​

​​cmakelists文件 ：​​

​​package.xml文件：​​

​​launch文件： ​​

​​运行测试：​​

---

效果如下：

​​https://www.bilibili.com/video/BV1jw411d7Fn/​​

四足机器人ROS+rviz仿真

直接上源码：

源码：

#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"
#include <math.h>
#define pi 3.141592653

int main(int argc,char* argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"gou2_test");
    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/Gou2/urdf/Gou2.urdf", my_tree))
    {
        ROS_ERROR("Failed to construct kdl tree");
    }
    else
    {
        ROS_INFO("成功生成kdl树!");
    }
    std::vector<std::string> joint_name = {"LF_JIAN_JOINT", "LF_ZHOU_JOINT", "LF_XI_JOINT", "LF_R_JOINT","LF_P_JOINT","LF_Y_JOINT"};
    std::vector<double> joint_pos = {0,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 = "LF_Y_LINK"; 
    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/6;
    q(2)=pi/6;
    q(3)=0;
    q(4)=0;
    q(5)=0;
    //定义末端4x4齐次变换矩阵
    KDL::Frame end_effector_pose;
    //定义逆运动学解算结果存储数组
    KDL::JntArray result;
    ros::Rate r(5);

    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);
    
    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] = q(0);
    joint_state.position[1] = q(1);
    joint_state.position[2] = q(2);
    joint_state.position[3] = q(3);
    joint_state.position[4] = q(4);
    joint_state.position[5] = q(5);
    joint_pub.publish(joint_state);
    fk_solver.JntToCart(q, end_effector_pose);//chushi

    KDL::Frame end_effector_pose1;//now
    fk_solver.JntToCart(q, end_effector_pose1);
    KDL::JntArray resu_last;//last
    resu_last=q;
    while(ros::ok())
    {
    //x=0.02*(t-sint);
    //y=0.02*(1-cost);
    for(int i=1;i<=20;i++)
    { 
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose1.p.data[0]=end_effector_pose.p.data[0]+x;
        end_effector_pose1.p.data[2]=end_effector_pose.p.data[2]+z;
        int rc = tracik_solver.CartToJnt(resu_last, end_effector_pose1, result);

        joint_state.header.stamp = ros::Time::now();
        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);

        resu_last=result;
        r.sleep();
    }
    

    for(int i=1;i<=20;i++)
    { 
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose1.p.data[0]=end_effector_pose.p.data[0]+0.005*2*pi-x;
        end_effector_pose1.p.data[2]=end_effector_pose.p.data[2];
        int rc = tracik_solver.CartToJnt(resu_last, end_effector_pose1, result);

        joint_state.header.stamp = ros::Time::now();
        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);

        resu_last=result;
        r.sleep();
    }
    }
    return 0;

    

       
}

cmakelists文件 ：

cmake_minimum_required(VERSION 2.8.3)



project(Gou2)



add_compile_options(-std=c++11)



find_package(catkin REQUIRED)



find_package(catkin REQUIRED COMPONENTS

  controller_manager

  gazebo_ros

  gazebo_ros_control

  joint_state_publisher

  robot_state_publisher

  roscpp

  rospy

  rviz

  xacro

  kdl_parser

  tf

  sensor_msgs

  std_msgs

  trac_ik_lib

)



find_package(Boost REQUIRED COMPONENTS date_time)

find_package(orocos_kdl REQUIRED)



find_package(roslaunch)



catkin_package(

#  INCLUDE_DIRS include

#  LIBRARIES quadruped_9g

   CATKIN_DEPENDS controller_manager gazebo_ros gazebo_ros_control joint_state_publisher robot_state_publisher roscpp rospy rviz xacro trac_ik_lib

   DEPENDS Boost orocos_kdl

)



include_directories(

  include

  ${catkin_INCLUDE_DIRS}

  ${orocos_kdl_INCLUDE_DIRS}

)



add_executable(gou2 src/gou2.cpp)

target_link_libraries(gou2 ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})











foreach(dir config launch meshes urdf)

  install(DIRECTORY ${dir}/

    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})

endforeach(dir)

package.xml文件：

<package format="2">

  <name>Gou2</name>

  <version>1.0.0</version>

  <description>

    <p>URDF Description package for Gou2</p>

    <p>This package contains configuration data, 3D models and launch files

for Gou2 robot</p>

  </description>

  <author>TODO</author>

  <maintainer email="[e-mail]" />

  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>controller_manager</build_depend>

  <build_depend>gazebo_ros</build_depend>

  <build_depend>gazebo_ros_control</build_depend>

  <build_depend>joint_state_publisher</build_depend>

  <build_depend>robot_state_publisher</build_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>rospy</build_depend>

  <build_depend>rviz</build_depend>

  <build_depend>xacro</build_depend>

  <build_depend>kdl_parser</build_depend>

  <build_depend>urdf</build_depend>

  <build_export_depend>controller_manager</build_export_depend>

  <build_export_depend>gazebo_ros</build_export_depend>

  <build_export_depend>gazebo_ros_control</build_export_depend>

  <build_export_depend>joint_state_publisher</build_export_depend>

  <build_export_depend>robot_state_publisher</build_export_depend>

  <build_export_depend>roscpp</build_export_depend>

  <build_export_depend>rospy</build_export_depend>

  <build_export_depend>rviz</build_export_depend>

  <build_export_depend>xacro</build_export_depend>

  <exec_depend>controller_manager</exec_depend>

  <exec_depend>gazebo_ros</exec_depend>

  <exec_depend>gazebo_ros_control</exec_depend>

  <exec_depend>joint_state_publisher</exec_depend>

  <exec_depend>robot_state_publisher</exec_depend>

  <exec_depend>roscpp</exec_depend>

  <exec_depend>rospy</exec_depend>

  <exec_depend>rviz</exec_depend>

  <exec_depend>xacro</exec_depend>

  <exec_depend>kdl_parser</exec_depend>

  <exec_depend>urdf</exec_depend>

  <depend>sensor_msgs</depend>

  <depend>std_msgs</depend>

  <depend>orocos_kdl</depend>

  <depend>trac_ik_lib</depend>

  <depend>boost</depend>

  <depend>roslaunch</depend>

  <depend>gazebo</depend>

  <export>

    <architecture_independent />

  </export>

</package>

launch文件： 

<launch>

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

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

  <node name="gou2" pkg="Gou2" type="gou2" 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>

运行测试：

roslaunch Gou2.launch