白泽六足机器人_ros_v1——单腿RVIZ仿真

导航在这里：ROS六足机器人导航贴

效果：

电脑有点卡，这个估计不是程序步态帧太少的原因。等我换个主机试试看。

六足机器人单腿运动学仿真（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 "tf2_geometry_msgs/tf2_geometry_msgs.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>
#include <serial/serial.h>
#include "geometry_msgs/PointStamped.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

#define pi 3.141592653

int main(int argc,char* argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"liuzu");
    ros::NodeHandle nh;
    ros::Publisher joint_pub=nh.advertise<sensor_msgs::JointState>("joint_states",1);
    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_ws3/src/LIUZU/urdf/LIUZU.urdf", my_tree))
    {
        ROS_ERROR("Failed to construct kdl tree");
    }
    else
    {
        ROS_INFO("成功生成kdl树!");
    }

    std::vector<std::string> joint_name = {
"RB_DATUI_JOINT", "RB_XI_JOINT", "RB_XIAOTUI_JOINT", "RB_R_JOINT","RB_P_JOINT","RB_Y_JOINT",
"RM_DATUI_JOINT", "RM_XI_JOINT", "RM_XIAOTUI_JOINT", "RM_R_JOINT","RM_P_JOINT","RM_Y_JOINT",
"RF_DATUI_JOINT", "RF_XI_JOINT", "RF_XIAOTUI_JOINT", "RF_R_JOINT","RF_P_JOINT","RF_Y_JOINT",
"LF_DATUI_JOINT", "LF_XI_JOINT", "LF_XIAOTUI_JOINT", "LF_R_JOINT","LF_P_JOINT","LF_Y_JOINT",
"LM_DATUI_JOINT", "LM_XI_JOINT", "LM_XIAOTUI_JOINT", "LM_R_JOINT","LM_P_JOINT","LM_Y_JOINT",
"LB_DATUI_JOINT", "LB_XI_JOINT", "LB_XIAOTUI_JOINT", "LB_R_JOINT","LB_P_JOINT","LB_Y_JOINT"
};

    std::vector<double> joint_pos = {
0,0,0,0,0,0,
0,0,0,0,0,0,
0,0,0,0,0,0,
0,0,0,0,0,0,
0,0,0,0,0,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_rb_end = "RB_Y_LINK"; 

    TRAC_IK::TRAC_IK tracik_rb_solver(chain_start, chain_rb_end, urdf_param, timeout, eps);
   
    KDL::Chain chain_rb;


    KDL::JntArray ll, ul; //关节下限, 关节上限
    bool valid = tracik_rb_solver.getKDLChain(chain_rb);
    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_rb);
    ROS_INFO("关节数量: %d", chain.getNrOfJoints());
    KDL::JntArray nominal(36);

    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)=0;
    q(2)=0;
    q(3)=0;
    q(4)=0;
    q(5)=0;
    
     //定义末端4x4齐次变换矩阵
    KDL::Frame end_effector_pose;
    //定义逆运动学解算结果存储数组
    KDL::JntArray result;
    ros::Rate r(5);

    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_rb_solver.CartToJnt(q, 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();
    
    joint_state.name.resize(36);
    joint_state.position.resize(36);
 
    for(size_t i = 0; i < 36; i ++)
    {
        joint_state.name[i] = joint_name[i];
        joint_state.position[i] = 0;
    }

    for(size_t i = 0; i < 6; i ++)
    {
        joint_state.name[i] = joint_name[i];
        joint_state.position[i] = result(i);
    }
    
    joint_pub.publish(joint_state);
    
    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_rb_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_rb_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;

}

launch文件：

<launch>

  <arg name="model" default="/home/zhitong/catkin_ws3/src/LIUZU/urdf/LIUZU.urdf"/>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find LIUZU)/rviz/urdf.rviz" />

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

  <node name="liuzu" pkg="LIUZU" type="liuzu" 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>

 编译成功之后，直接运行launch文件即可。