基于雅可比行进轨迹

# velocity_transform specified in m/s as relative motion
def move(velocity_transform):
  t_start = time.time()
  pose_start = effector.GetTransform()
  while True:
    t_now = time.time()
    t_elapsed = t_now - t_start
    pose_current = effector.GetTransform()
    translation_target = pose_start[:3,3] + velocity_transform[:3,3] * t_elapsed
    v_trans = translation_target - pose_current[:3,3]
    vels = J_plus.dot(v_trans) # some math simplified here

rot_t1 = np.dot(pose_start[:3,:3], velocity_transform[:3,:3]) # desired rotation of end-effector 1 second from start
quat_start = quatFromRotationMatrix(pose_start) # start pose as quaternion
quat_t1 = quatFromRotationMatrix(rot_t1) # rot_t1 as quaternion

# use SLERP to compute proper rotation at this time
quat_target = quatSlerp(quat_start, quat_t1, t_elapsed) # world_to_target
rot_target = rotationMatrixFromQuat(quat_target) # world_to_target
v_rot = np.dot(np.linalg.inv(pose_current[:3,:3]), rot_target) # current_to_target
v_euler = eulerFromRotationMatrix(v_rot) # get rotation about world axes

#1 楼

在Sciavicco-Siciliano的书中很好地描述了处理定向误差（就所需位置和所需速度而言的误差而言）所需的数学运算。请参阅从第137页开始的第3.7.3节。

我个人使用轴角标记，因此我对第139页给出的公式（3.89）进行了编码，结果正确。

简而言之，给定在位置和速度上都跟踪笛卡尔空间中6D轨迹的目标，输入是平移部分的$ \ left（p_d，\ dot {p} _d \ right）$和$ \向左（\ theta_d，\ omega_d \ right）$表示方向部分（轴角度）。输入是随时间变化的（即$ p_d \ equiv p_d \ left（t \ right）$）。然后，分析误差的动态（例如参见第3.7.1节），得出一个收敛规则，其$ \ mathbin {K} _P $和$ \ mathbin {K} _O $是作用于正定矩阵的增益错误本身。这些矩阵仅设置收敛速度。

我没有进入您的代码，但是此公式作为其不同版本，利用Jacobian的伪逆来产生阶跃。逐步以大约指定的笛卡尔速度将机械手朝末端执行器的目标位置和方向驱动所需的关节速度。

更新

请在下面的注释中，为您提供一个非常基本的起点，请看一下这些行。它们是计算笛卡尔误差的方法的一部分，而笛卡尔误差又是我们与iCub一起使用的操作空间控制器的重要组成部分。

现在想象一下。然后将执行所有行ctrlPose==IKINCTRL_POSE_FULL。第一块计算平移误差，而第二块实现上面的公式（3.84），该公式处理轴角表示法中的方向误差，其中矩阵Des表示以DCM格式转换的所需方向（Direction-Cosine-Matrix，

一旦正确地填充了$ 6 \ times 1 $误差向量$ e $-是后三个分量，则表示当前姿势。轴-角度约定中的方向错误-然后可以运行控制定律：

$
\ dot {q} = J_G ^ + \ cdot e，
$

其中，$ J_G $是联合速度的$ 6 \ times n $几何雅可比矩阵，而$ \ dot {q} $是$ n \ times $ 1的联合速度矢量。在此基本实现中，我们不处理目标方位速度$ \ omega_d $，它们为零，也不处理目标平移速度$ \ dot {p} _d $。

此外，我们知道$ e = \ left [e_P，e_O \ right] ^ T $，但我们也可以施加$ e = \ left [K_P \ cdot e_P，K_O \ cdot e_O \ right] ^ T $以收敛速度播放。

开始时，甚至可以使$ K_P $和$ K_O $的值大于$ I $，但是请记住，伪逆算法由于例如雅可比矩阵的奇异性和增益矩阵的高值可能会加剧此问题。

要缓解这种不利影响，可以在雅可比矩阵的伪逆矩阵（非常快，但容易产生不稳定）和噪声之间实现协同作用。 Jacobian换位（慢得多，但本质上稳定），最后以经典的阻尼最小二乘（DLS）技术（又称为Levenberg-Marquardt算法）结束。

最后，DLS还会遭受可靠性，准确性和速度问题，尤其是当您要同时控制配备许多自由度的非常冗余的机械手的平移和方向时，尤其如此。这就是为什么我最终想出使用能够即时解决逆运动学问题的非线性优化器的原因。一旦在配置空间中也知道了所需的姿势，就可以极大地简化控制器的设计。

#2 楼

Ugo的答案是“ Sciavicco-Siciliano”，这也是一本好书，我也会引用。

第3.6章介绍了所谓的解析雅可比行列式，它与所谓的几何雅可比行列式不同它显示在：
$
\ omega = J_ {geom} \ cdot \ dot {q}，
$
，但是必须从$ J_ {geom} $获得借助转换矩阵$ T_A $：

$
J_ {analytical} = T_A \ cdot J_ {geom}
$

或更翔实的是：

$
\ dot {\ phi} = J_ {analytical} \ cdot \ dot {q}
$

我允许我自己是本章的一句话：


从物理角度看，$ \ omega_e $的含义比$ \ dot {\ phi} _e $的含义更直观
。 。 ωe的三个分量表示相对于基础框架的角速度的
分量。
相反，$ \ dot {\ phi} _e $的三个元素表示角的非正交
分量相对于
框架的轴定义的速度随末端执行器方向的变化而变化。另一方面，虽然$ \ dot {\ phi} _e $随时间的积分为$ \ phi_e $，但
$ \ omega_e $的积分并不容许物理上明确的
解释...


所以好消息是我们可以使用$ \ phi_e $进行工作-以欧拉角表示的末端执行器的方向，坏消息是转换矩阵$ T_A $引入了奇点，这使2的作者放弃了分析雅可比行列式的概念。

OpenRave出现了：

manip=robot.GetManipulators()[0]
# we calculate the rotational Jacobian in quaternion space
quatJ = manip.CalculateRotationJacobian()
quatManip0 = manip.GetTransformPose()[0:4]
# order of multiplication may be wrong in the next line
quatManipTarget = qmult(quatManip0, deltaQuat) # 
wSol=np.linalg.lstsq(quatJ, quatManipTarget)

因此，通过在四元数空间中使用解析雅可比矩阵，可以避免变换矩阵$ T_A $的奇异性。您的Jacobian仍然可以具有数值不稳定性，但这是不同的。

更新：
我再次考虑了四元数空间中$ T_A $的奇异性和解析雅可比行列的可能数值不稳定性。确实，我先前关于$ \ mathbb {R} ^ 3 $中的奇异性与四元数空间中的数字不稳定性不同的主张太“强烈”。

#3 楼

因此，我研究了quatSlerp函数，似乎您传递的t_elapsed错误。看起来该参数应该是插值值，如果t = 0，则返回的旋转是恒等式；如果t = 1，则返回的旋转是所传递的前两个四元数参数之间的完整旋转。

如果此代码在while循环内，并且t_elapsed大于1，则手臂将变得不稳定。

这不能解释为什么不稳定的行为会受到位置的影响。

希望这不是调试代码的意思。

quatSlerp