Mini Cheetah 代码分析(八)基于零空间的任务分级

一、主要公式

二、源代码注释

三、相关原理解释

一、主要公式

二、源代码注释

该功能的实现在文件KinWBC.cpp中的FindConfiguration函数,主要看注释,与公式是能够对应起来的,由第0个任务,也就是接触任务开始进行迭代,最终求出delta_q和qdot。qddot在另外一个文件WBIC.cpp中

#include "KinWBC.hpp"
// #include <Utilities/Utilities_print.h>
#include "PseudoInverse.h"template <typename T>
KinWBC<T>::KinWBC(size_t num_qdot): threshold_(0.001), num_qdot_(num_qdot), num_act_joint_(num_qdot - 6)
{I_mtx = DMat<T>::Identity(num_qdot_, num_qdot_);
}template <typename T>
bool KinWBC<T>::FindConfiguration(const DVec<T> &curr_config, const std::vector<Task<T> *> &task_list,const std::vector<ContactSpec<T> *> &contact_list, DVec<T> &jpos_cmd,DVec<T> &jvel_cmd) //
{// Contact Jacobian Setup // contact任务的雅可比,堆叠成一个大矩阵DMat<T> Nc(num_qdot_, num_qdot_); // dotx=Jc*qdot,接触雅可比矩阵,维度为仿真std::cout << "num_qdot_:zhhw \n"<< num_qdot_ << std::endl;Nc.setIdentity();if (contact_list.size() > 0){DMat<T> Jc, Jc_i;contact_list[0]->getContactJacobian(Jc);size_t num_rows = Jc.rows();for (size_t i(1); i < contact_list.size(); ++i){contact_list[i]->getContactJacobian(Jc_i);size_t num_new_rows = Jc_i.rows();Jc.conservativeResize(num_rows + num_new_rows, num_qdot_);Jc.block(num_rows, 0, num_new_rows, num_qdot_) = Jc_i; // 添加这个contact任务的雅可比num_rows += num_new_rows;}// Projection Matrix_BuildProjectionMatrix(Jc, Nc); // 对应公式20, 构建Jc雅可比矩阵的零空间矩阵Nc}// First TaskDVec<T> delta_q, qdot;DMat<T> Jt, JtPre, JtPre_pinv, N_nx, N_pre;Task<T> *task = task_list[0];task->getTaskJacobian(Jt);JtPre = Jt * Nc;                   // 对应公式19,由Nc乘以当前任务的雅可比矩阵Jt_PseudoInverse(JtPre, JtPre_pinv); // 对应公式16中的系数矩阵delta_q = JtPre_pinv * (task->getPosError()); // 对应公式16,delt{q_0}=0qdot = JtPre_pinv * (task->getDesVel());DVec<T> prev_delta_q = delta_q; // 计算出delt{q_1}DVec<T> prev_qdot = qdot;// 当前任务计算完成//  计算下一层级任务所需的变量,For the next task_BuildProjectionMatrix(JtPre, N_nx); // 对应公式20, 构建JPre雅可比矩阵的零空间矩阵N_nxN_pre = Nc * N_nx;                   // 对应公式19,第二步,从N0开始,for (size_t i(1); i < task_list.size(); ++i) // 从i=1开始迭代{task = task_list[i];task->getTaskJacobian(Jt); // 当前任务的雅可比矩阵JtJtPre = Jt * N_pre;        // 对应公式19,构建JPre_PseudoInverse(JtPre, JtPre_pinv); // JtPre矩阵的伪逆delta_q =prev_delta_q + JtPre_pinv * (task->getPosError() - Jt * prev_delta_q); // 对应公式16,求出速度增量delta_qqdot = prev_qdot + JtPre_pinv * (task->getDesVel() - Jt * prev_qdot);      // 对应 公式17,求出下一次迭代的qdot// 当前任务计算完成//  计算下一层级任务所需的变量,For the next task_BuildProjectionMatrix(JtPre, N_nx); // 当前任务的零空间矩阵N_nxN_pre *= N_nx;                       // 由最高层级第0个的零空间乘以当前的Npre得到当前的零空间,开始迭代,prev_delta_q = delta_q;              // 当前的delta_q用于下一次迭代prev_qdot = qdot;                    // 当前的qdot用于下一次迭代}for (size_t i(0); i < num_act_joint_; ++i){jpos_cmd[i] = curr_config[i + 6] + delta_q[i + 6]; // 将数据发送出去jvel_cmd[i] = qdot[i + 6];}return true;
}template <typename T>
void KinWBC<T>::_BuildProjectionMatrix(const DMat<T> &J, DMat<T> &N)
{DMat<T> J_pinv;_PseudoInverse(J, J_pinv);N = I_mtx - J_pinv * J; // 对应公式20,N=1-(Jc# * Jc)求解零空间
}template <typename T>
void KinWBC<T>::_PseudoInverse(const DMat<T> J, DMat<T> &Jinv)
{pseudoInverse(J, threshold_, Jinv); // 计算矩阵的伪逆矩阵
}template class KinWBC<float>;
template class KinWBC<double>;

三、相关原理解释

先通过一个简单的例子进行说明

基于零空间方法(NUB)的全身控制(WBC)的简单实现 - 知乎

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/bicheng/12947.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Java类和对象(二)—— 封装,static 关键字与代码块

前言 在面向对象的编程语言中&#xff0c;有三大特性&#xff1a;封装、继承和多态~~ 今天我们就来学习封装的知识 封装 什么是封装 在现实生活中&#xff0c;我们经常使用手机来进行沟通与交流&#xff0c;实际上我们拿到的手机是被封装好的&#xff0c;精美的屏幕&a…

关键字详解

1.用于定义访问权限修饰符的关键字 面向对象程序三大特性&#xff1a;封装、继承、多态。 1.1 访问权限符 Java 中主要通过类和访问权限来实现封装&#xff1a; 类可以将数据以及封装数据的方法结合在一起 &#xff0c;更符合人类对事物的认知&#xff0c;而访问权限用来控制…

5月15日,机器人任务挑战赛(无人协同系统)第二期培训即将开启!

一.大赛培训通知 本月起&#xff0c;卓翼飞思实验室将针对机器人任务挑战赛&#xff08;无人协同系统&#xff09;赛项内容开启赛事培训计划&#xff0c;采用“线上线下”相结合的培训模式&#xff0c;围绕赛事关键技术&#xff0c;让您轻松应对比赛。本期培训为第二期&#x…

Go微服务: 日志系统ELK核心架构设计

微服务日志系统建设 1 &#xff09;为什么需要日志系统 业务发展越来越庞大&#xff0c;服务器越来越多各种访问日志&#xff0c;应用日志&#xff0c;错误日志量越来越多&#xff0c;无法管理开发人员排查问题&#xff0c;需要到服务器上查日志 2 &#xff09;Elastic Stack…

SpringBean详解

文章目录 概述Spring获取Bean的流程依赖注入bean的作用域Spring 中的 Bean 是线程安全的吗Spring如何处理线程并发问题bean 的自动装配和方式Resource和Autowired的区别bean的自动装配bean的生命周期BeanFactoryBeanFactory 常用的实现类有哪些BeanFactory与FactoryBean的不同A…

惠普打印机无线网络连接设置

休息一下&#xff0c;灌个水。这次没多少内容&#xff0c;具体步骤惠普官网上都有&#xff0c;唯一增加的是对安装过程中踩的坑做了一个说明。 一&#xff0e;打印机无线网络连接设置步骤 惠普打印机设置无线网络连接&#xff0c;共16个步骤。 1. 在电脑上打开任意浏览器&am…

HAProxy系列文章二《Patroni+ETCD+PG14+HAProxy的安装部署》

瀚高数据库 目录 文档用途 详细信息 文档用途 本文主要介绍Patroni架构下单点HAProxy的安装部署&#xff0c;通过单点HAProxy实现数据库的负载均衡。本文为HAProxy系列文章之一&#xff0c;其他相关文章请点击文档下方的相关文档链接进行详细查看&#xff0c;文章内不在赘述。…

Spring MVC(四) 数据校验

在开发过程中有一环必不可少的部分就是数据校验&#xff0c;用户在页面中填写的数据通过表单提交时&#xff0c;前端的JS可以做一些是否合法性的验证&#xff0c;比如是否为空、两次密码是否一致、格式是否正确等等验证。当数据到了后台控制器&#xff0c;为了确保程序的健壮性…

Python专家编程系列: 11.为什么在python使用__name__ == ‘__main__‘

0. 标题 Python专家编程系列: 11.为什么在python使用__name__ ‘main’ id: 104 作者: quantgalaxyoutlook.com 欢迎交流 1. 单个 python 文件中的 __name__ 是什么&#xff1f; 我们先看一个简单的python程序脚本a.py: # a.pyprint(__name__) # python a.py # _…

最直接解决 element + sortablejs 拖拽后顺序错误

直接按索引对调两行对象&#xff0c;免加row-key const oldRow this.valueMy[evt.oldIndex] this.valueMy[evt.oldIndex] this.valueMy[evt.newIndex] this.valueMy[evt.newIndex] oldRow

内网环境ubuntu设置静态ip、DNS、路由,不影响网络访问

内网环境通常是有线的&#xff0c;通过服务器的ip、mac、dns地址访问网络才生效的&#xff0c;如果ip地址变了&#xff0c;就不能访问网络了。 如果你的ip地址变了&#xff0c;或者要防止ip变更影响网络访问&#xff0c;就要设置 1、依次点击右上角的电源-设置&#xff0c;在打…

linux 正则表达式+文本三剑客

一、正则表达式 1.正则表达式的概念 REGEXP&#xff1a; Regular Expressions&#xff0c;由一类特殊字符及文本字符所编写的模式&#xff0c;其中有些字符&#xff08;元字符&#xff09;不表示字符字面意义&#xff0c;而表示控制或通配的功能&#xff0c;类似于增强版的通…

公司里的“卷王”,是主动卷还是迫于无奈?

先来唠唠 “卷” 这个词是近几年流行起来的网络用语&#xff0c;它是内卷的简化形式&#xff0c;“内卷”本来是一个名不见经传的普通词语&#xff0c;经网络流传&#xff0c;很多人就用其来指代非理性的内部竞争或“被自愿”竞争。 在现代职场&#xff0c;有一群人被戏称为&…

《大学数学3(第三版)》

文章目录 [toc]第一章&#xff1a;行列式第一节|方程组与行列式二元线性方程组和二阶行列式 第二节| n n n阶行列式排列逆序数对换例题 1 1 1 n n n阶行列式 第三节|行列式的性质和计算性质 1 1 1性质 2 2 2&#xff1a;互换 n n n阶行列式的任意两行&#xff08;列&#xff09…

机器学习算法那些事 | 60个“特征工程”计算函数(Python代码)

本文来源公众号“机器学习算法那些事”&#xff0c;仅用于学术分享&#xff0c;侵权删&#xff0c;干货满满。 原文链接&#xff1a;60个“特征工程”计算函数&#xff08;Python代码&#xff09; 近期一些朋友询问我关于如何做特征工程的问题&#xff0c;有没有什么适合初学…

【无标题】进程池/Linux

#include <iostream> #include <vector> #include <unistd.h> #include <cassert> #include <sys/types.h> #include <string> #include <sys/wait.h> // using namespace std; #include "Tash.hpp" class channel//封装文…

三分钟快速上手SpringSecurity框架

导入依赖框架 web 框架(spring-boot-starter-web) <dependency><groupId>org.springframework.boot</groupId><artifactId>spring-boot-starter-web</artifactId></dependency> springSecurity 框架(spring-boot-starter-security) <de…

递归-常规问题详解

目录 前言 递归经典题目 子集 77. 组合 46. 全排列 前言 递归在计算机算法中有很重要的地位&#xff0c;它可以解决条件具有重复性的问题。我们在快速排序和归并排序&#xff0c;都是利用了递归去解决问题的。写好一个递归代码不是太容易&#xff0c;很容易造成死循环最终…

基于单片机的空气质量检测系统设计(51+4G版)-设计说明书

设计摘要&#xff1a; 本设计是基于单片机的空气质量检测系统设计涉及以下主要功能&#xff0c;旨在监测甲烷和一氧化碳的浓度&#xff0c;并在浓度过高时采取相应措施&#xff0c;以确保室内空气质量的安全。该系统使用传感器对甲烷和一氧化碳的浓度进行检测。传感器将收集到…

人物介绍模板 PSD 源文件免费获取

免费获取 下载链接在最后&#xff01; 下载链接在最后&#xff01; 下载链接在最后&#xff01; 下载链接在最后&#xff01; 下载链接在最后&#xff01; 链接&#xff1a;https://pan.baidu.com/s/1sq3e6djMdZt76Sh_uqVxWg 提取码&#xff1a;naun