basic_env.xml：提取各环境相通性，每个实验的xml都会用到
包括一些可视化、环境美化、接触使能
robot_stick.xml：用于恒力接触实验，连杆、力传感器共长0.169m
robot_link.xml：用于开关抽屉、门实验，连杆、力传感器共长0.169m
两个文件的区别在于dummy body不一样，第一个是球，第二个是卡子
末端有三个位置约束:stick位置与长度,ee位置，dummy body位置，因此一共4个数，记得一次全部修改
jk5_table_v1为原版文件
jk5_table_v2为改进文件，提取机器人部分，优化xml结构
jk5_table_v3为测bug文件
jk5_door_v1为松耦合文件，只能关门
jk5_opendoor_v1为紧耦合文件，能开门与关门
两者均被废弃，可以用，但是没用来训练，因为实际机器人没法那么摆
根据cabinet设计的原尺寸箱子。
jk5_cabinet_v1用于恒力接触任务，更近更低
jk5_cabinet_v2用于开关抽屉、门实验 ，更高更远
todo:关节、门、抽屉的属性设置
每一个时刻的位姿（3+4）、速度（3+3）、加速度（3+3）
位置规划与姿态规划完全独立，分别确定位姿、速度、加速度中的一部分
面积为1的曲边梯形，全过程为：加速-匀速-减速
加速、减速过程的起始、结束速度为0
总时间、加速时间二者能唯一确定曲线参数
求得世界坐标系下，起点、终点位姿之差即可
- 加速度：与速度微分曲线一致，乘以位移变化量
- 速度：与速度曲线一致，乘以位移变化量
- 位置：与速度积分曲线一致，乘以位移变化量
将世界坐标系下的圆弧转换为平面坐标系下的圆弧
- 加速度：求平面圆周运动的加速度，转换到空间坐标系
- 速度：求平面圆周运动的速度，转换到空间坐标系
- 位置：求平面圆周运动的位置，转换到空间坐标系
- 加速度：角加速度与四元数微分的关系
- 加速度：角速度与四元数微分的关系
- 姿态：四元数插值
没啥用。只在求解连接点的时候用到正逆运动学，来计算初始的关节角度；强化学习没用到kdl模型，转pickle还有问题，因此注释掉
针对不同任务，kdl model有不同的连杆长度
现在有五个坐标系，以及之间的转换
世界坐标系：世界的中心的位姿
机器人基坐标系：机器人的baselink的位姿
机器人末端坐标系：不带力传感器、杆的机器人末端，标记为re
机器人连杆坐标系：杆的末端的位姿，标记为ee，当前安装有connect force sensor，当前雅克比基于该坐标系，运动学通过此定义
机器人工具坐标系：末端用于碰撞的dummy body坐标系
把手是突出的，记得计算把手的位置 当前轨迹规划（期望轨迹）的是相对于末端连杆/力传感器的 初始角度用末端连杆/力传感器算出
基于旋转矩阵的旋转矩阵误差
误差趋于0001（实数在最后）则说明很好，说明转动角度为0
基于旋转矩阵的四元数误差
基于四元数的四元数误差
控制都是结合了机器人动力学，具体在于q两点的计算不同
计算力矩控制器，算得是q两点+d q一点+p q
阻抗控制器，算得是用阻抗方程结算出来的q两点+kd q一点+kp q+外力
上面的没有外力，下面的有外力，似乎是唯一区别
阻抗控制中的mbk，结合可以得到计算力矩控制器中kpkd的相同形式，但是多考虑了接触力，改变了稳定点
所以阻抗控制没有pd参数，导纳控制有pd参数
计算力矩控制中pd参数引入的好处：实现轨迹跟踪
用到了外力
e = xc - xd
M ddot(e) + B dot(e) + K ddot(e) = F
各方向阻抗特性解耦，形变量与弹力一致
现在有完全基于旋转矩阵的阻抗控制、完全基于四元数的导纳控制、完全基于旋转矩阵的导纳控制
性能区别不大，完全基于旋转矩阵的导纳控制收敛快于完全基于旋转矩阵的导纳控制
因此最大空间/关节速度也更大，最大关节力矩也更大(500与300)，瞬间接触力也更大(130与100)
viewer与logger主要用于测试环境时使用
Jk5StickStiffnessEnv单独也为gym格式,循环为状态s_t
env在其基础上封装，仍然是gym格式，循环为观测o_t
env为Jk5StickStiffnessEnv与gym的中间件,o_t由s_t组成
todo：算法对比
todo：默认网络模型,rnn
todo：超参数优化
当前为线位置、四元数的姿态、线速度、角速度 四元数的连续性处理：就是啥也不要处理，不要将实部强制转换为正，多此一举 并没有影响稳定性，而且还连续了
均输出为-1到1，通过不同的缩放到不同的维度：
不选择直接输出动作，有效避免shaky behavior
0<\epsilon<1时，系统的输出存在超调量，且\epsilon决定了超调量的大小
超调量只与\epsilon有关，\epsilon和\omega_{n}共同决定了系统的响应速度。
\epsilon越大超调量越小，响应速度越慢。
\omega_{n}越大系统响应越快。
选择MB均为K的函数，使得动态性能恒定
6维：刚度改变量逐渐添加到刚度上
本来单位就不一样，维度不一样也是合理的
旋转刚度异常导致任务失败，但是实际上所有任务都不太需要修改旋转刚度，所以把他变化范围降低
幅度确定：单次力改变范围，全局力改变范围
f=kx，x一般为0.017左右，浮动范围不超过0.001
单次力改变范围：0.017×150=2.55N
全局力改变范围80×2.55=204N
7维：位置改变量+姿态改变量（旋转轴+角度，旋转轴需要单位化，然后转换到旋转矩阵与四元数）
位姿改变量将统一改变两个动作之间的所有位姿，不改变速度与加速度
上一时刻的位姿变化不会影响下一时刻，不存在叠加问题，每次的轨迹都在生成的轨迹附近 除非跟刚度的处理一样，将所有的动作叠加
4维（旋转轴+角度，旋转轴需要单位化，然后转换到旋转矩阵与四元数）
动作直接为接下来一段时间的刚度椭圆姿态，既没有插值，又没有叠加
todo：权重优化
时间标识 刚度标识 接触力标识：刚度约束不直观
位置误差
姿态误差：
xquat描述了末端姿态相对于基坐标系的四元数旋转，确实没问题，就是xyzw看起来挺迷糊
orientation是最直观的误差，在哪个方向上加力就产生哪个方向的偏差
任务的设计，未知信息，当前利用的信息，优化目标，轨迹规划 不同任务中哪些部分是一定需要优化的，哪些是不需要优化的
任务的优化目标：
轨迹优化：当前轨迹全是用已知量做的，改成未知量
力优化：使得力全在有效方向，因此与任务挂钩、而不是与机器人挂钩
0 0 30的接触力是只有当刚度椭球与桌面平行才能达到的，否则在x方向上一定存在着分力
因此存在唯一的最优刚度椭球，其他所有任务绝对无法超过此奖励
后倾3度
0 numpy.sin(3/180numpy.pi/2) 0 numpy.cos(3/180numpy.pi/2)
末端垂直接触面，恒力接触 一定要垂直吗
优化期望轨迹（期望轨迹有向下趋势，或让桌子有点倾角）、接触力（正比于frictionloss） 优化打开距离，关闭距离 刚度也需要优化（不能太小），不然就是那种蹦蹦跳跳的 接触点可以左右移动
锁住能够降低震荡，基本无震荡
r = np.sqrt(0.21752+0.4752) theta = np.arctan(0.475/0.2175) x=0.8-rnp.cos(theta-np.pi/60) y=-0.1135 z=0.2+rnp.sin(theta-np.pi/60)
0.5579384969754818,-0.1135,0.6629659585255823
env.inverse_kinematics(env.qpos_init_list,np.array([x,y,z]), np.array([0, 0, 1, 1, 0, 0, 0, 1, 0]))
开门时估计半径大于实际半径可保一直接触
关门时估计半径小于实际半径可保一直接触
todo：任务轨迹实现
r = np.sqrt(0.21752+0.222) theta = np.arctan(0.22/0.2175) x=0.8-rnp.cos(theta-np.pi/60) y=-0.1135 z=0.2+rnp.sin(theta-np.pi/60)
0.5712841658174326,-0.1135,0.40831542716316604
damping与contact：会影响接触力
全为0的话，输入完美轨迹能够保证轨迹误差小于0.5mm，也就验证了轨迹规划与门把手实际运动弧线的一致性
此外，当全为0时，除了加速与减速阶段，其余阶段接触力很小（0.05N），也就是说，开门的力靠着变速运动的门的质量，以及关节的阻尼摩擦撑着
任务对大刚度接受能力强，对小刚度接受能力弱
为啥要执着于一种相对位置，为啥要执着于一种末端
多个环境，多种末端连接器
紧耦合：没法让他利用惯性移动
尽量 做仿真吧，先别管实物结构
第一个任务做实物，余下的做仿真算了，那两个任务紧耦合很难做 第一个有末端连杆的实物 第二三个还要设计紧耦合机制：钩子？自己画一个钩子，取消约束 既然只做仿真，那就随便改，只要验证有效性，改这个重新训练难度不会特别大 多个xml就多个吧，自己注意一点，
如何减少运算时间消耗，删除多余代码？
任务简化，不要太复杂
noslip_iterations="20"严重降低两个任务效果
在接触任务引发无法解释的接触力
在开抽屉任务引发震荡
力相对于哪个坐标系