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

源码：

整体项目文件地址：

先看效果：

白泽四足机器人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.141592653int 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;sensor_msgs::JointState joint_state;std::string robot_desc_string;nh.param("robot_description", robot_desc_string, std::string());KDL::Tree my_tree;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",
"LB_JIAN_JOINT", "LB_ZHOU_JOINT", "LB_XI_JOINT", "LB_R_JOINT","LB_P_JOINT", "LB_Y_JOINT",
"RF_JIAN_JOINT", "RF_ZHOU_JOINT", "RF_XI_JOINT", "RF_R_JOINT","RF_P_JOINT", "RF_Y_JOINT",
"RB_JIAN_JOINT", "RB_ZHOU_JOINT", "RB_XI_JOINT", "RB_R_JOINT","RB_P_JOINT", "RB_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
};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 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_pose1_l;KDL::Frame end_effector_pose1_r;//定义末端4x4齐次变换矩阵(各个关节的实时末端齐次矩阵)KDL::Frame end_effector_pose_l;//left nowKDL::Frame end_effector_pose_r;//left now//定义逆运动学解算结果存储数组KDL::JntArray result_l;//leftKDL::JntArray result_r;//right//上一帧的关节变量KDL::JntArray resu_last_l;//left lastKDL::JntArray resu_last_r;//right lastros::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_pose1_l);fk_solver.JntToCart(q,end_effector_pose1_r);ROS_INFO("更新关节状态");joint_state.header.stamp = ros::Time::now();//ROS_INFO("%d",joint_state.header.stamp);joint_state.name.resize(24);joint_state.position.resize(24);for(size_t i=0;i<=23;i++){joint_state.name[i] = joint_name[i];}for(int i=0;i<=23;i++){joint_state.position[i]=q(i%6);}joint_pub.publish(joint_state);fk_solver.JntToCart(q, end_effector_pose_l);fk_solver.JntToCart(q, end_effector_pose_r);resu_last_l=q;resu_last_r=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_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);joint_state.header.stamp = ros::Time::now();for(int j=0;j<=5;j++){joint_state.position[j]=result_l(j);}for(int j=6;j<=17;j++){joint_state.position[j]=result_r(j%6);}for(int j=18;j<=23;j++){joint_state.position[j]=result_l(j%6);}
/*for(int j=0;j<=23;j++){joint_state.position[j]=((j/6)==(0||3))?result_l(j%6):result_r(j%6);}
*/joint_pub.publish(joint_state);resu_last_l=result_l;resu_last_r=result_r;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_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+0.005*2*pi-x;end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);joint_state.header.stamp = ros::Time::now();for(int j=0;j<=5;j++){joint_state.position[j]=result_l(j);}for(int j=6;j<=17;j++){joint_state.position[j]=result_r(j%6);}for(int j=18;j<=23;j++){joint_state.position[j]=result_l(j%6);}
/*for(int j=0;j<=23;j++){joint_state.position[j]=((j/6)==(0||3))?result_l(j%6):result_r(j%6);}
*/joint_pub.publish(joint_state);resu_last_l=result_l;resu_last_r=result_r;r.sleep();}}return 0;
}

整体项目文件地址：

https://gitee.com/li9535/arduino-baize-quadruped-robot/tree/master/Gou2

白泽四足机器人ROS+rviz仿真（二）整体行走步态相关推荐

Arduino白泽四足机器人——matlab逆运动学求解
基于"白泽"四足机器人足端轨迹的插值方法目录 1 应用背景 2 模型建立 3 MATLAB对足端轨迹进行拟合插值 4 结果与分析 5 matlab逆运动学程序 1 应用背景近年 ...
matlab四足仿真,基于MATLAB的四足机器人建模与仿真.docx
摘要:本课题讨论了一种利用MATLAB中Robotics Toolbox对机器人进行的仿真建模的技术,对四足机器人进行行仿真建模.通过设计确定主要研究对象为哺乳类四足机器人.确定了机器人的腿部关节结构 ...
白泽四足机器人Opencat玩法之——校准关节和陀螺仪
导航在这里:白泽四足机器人导航贴关节校准一定要连接电池! 基于上面已经上传了WriteInstinct.ino程序的基础上,打开串口监视器,可以看到如下输出: 注意:下方红框两个参数要设置与图片一致 ...
【四足机器人】从零开始搭建四足机器人mini cheetah仿真环境（零）准备工作
文章目录一.准备工作 1.pybullet 2.mini cheetah 的模型 3.urdf文件解析一.准备工作 1.pybullet 我们的仿真环境基于pybullet,因此需要先安装好pyb ...
四足机器人|机器狗|仿生机器人|多足机器人|Adams仿真|Simulink仿真|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货！需要资料及指导的可以联系我！
四足机器人|机器狗|仿生机器人|多足机器人|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货!需要资料及指导的可以联系我!QQ:1096474659 基于CPG的四 ...
四足机器人——12自由度舵机狗DIY（二）
目录一.四足机器人步态研究控制的现状 1.1目前的三种控制策略 <1>基于静态稳定的控制方法. <2>基于动力学模型的控制方法. <3>基于生物所具有的神经性调节 ...
cpg 控制算法一种python实现（参考《仿生四足机器人技术》）
参考<仿生四足机器人技术> 使用python实现四足机器人CPG控制网络二,即xi作为髋关节输出,对yi输出整型作为膝关节输出. 因为没有经验,不知道输出信号是否正确,发到网上希望能够得到 ...
相对全面的四足机器人驱动规划MATLAB和Simulink实现方式（足端摆线规划，Hopf-CPG，Kimura-CPG）
许久没更新四足机器人相关的博客文章,由于去年一整年都在干各种各样的~活,终于把硕士毕业论文给写好,才有点时间更新自己的所学和感悟.步态规划和足端规划只是为了在运动学层面获取四足机器人各关节的期望角位移 ...
四足机器人 2.建模和步态规划
1.建模四足机器人建模: 运动学建模和动力学建模四足机器人在运动过程中,与所处环境进行交互作用,为提高机器人运动的稳定性和适应性,需要整体考虑四足机器人的动力学模型.足-地接触模型和步态生成与变换 ...

白泽四足机器人ROS+rviz仿真（二）整体行走步态

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

源码：

整体项目文件地址：

白泽四足机器人ROS+rviz仿真（二）整体行走步态相关推荐

最新文章

热门文章