impedance control with visp_ros
代码
先把代码贴上来，后面进行总结，同时结合chatgpt记录一下其中的数学原理。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140#include <visp_ros/vpROSRobotFrankaCoppeliasim.h>
#include "base_tool_func.h"
symc::SystemInfo sys_info;
int main(int argc, char** argv){
symc::OptSettings opt_settings;
if(symc::ArgHandle(argc, argv, opt_settings) == 1){
return EXIT_SUCCESS;
}
symc::SignalManager::RegisterSignalHandlers();
vpROSRobotFrankaCoppeliasim robot;
try{
ros::init(argc, argv, "visp_ros", ros::init_options::NoSigintHandler);
ros::NodeHandlePtr n = boost::make_shared<ros::NodeHandle>();
ros::Rate loop_rate(1000);
ros::spinOnce();
robot.setVerbose(opt_settings.verbose);
robot.connect();
symc::ResetSimulation(robot);
vpColVector q_init( { 0, vpMath::rad( -45 ), 0, vpMath::rad( -135 ), 0, vpMath::rad( 90 ), vpMath::rad( 45 ) } );
symc::InitRobotPosition(robot, q_init);
vpColVector q(7,0); // joint position
vpColVector q_dot(7,0); // joint velocity
vpColVector x_error(6,0); // position error in cartesian space
vpColVector x_dot_error(6,0); // velocity error in cartesian space
vpColVector x_dot_desired(6,0); // desired cartesian velocity
vpColVector x_dot_dot_desired(6,0); // desired cartesian acceleration
vpColVector C(7,0); // Coriolis and centrifugal forces
vpColVector F(7,0); // friction forces
vpColVector tau0(7,0); // initial joint torque
vpColVector tau(7,0); // joint torque
vpColVector tau_desired(7,0); // desired joint torque
vpColVector tau_cmd(7,0); // joint torque command
vpMatrix fJe(6,7); // Jacobian matrix in operational frame
vpMatrix Ja(6,7); // Jacobian matrix in world frame
vpMatrix Ja_dot(6,7); // Jacobian matrix derivative
vpMatrix Ja_old(6,7); // old Jacobian matrix
vpMatrix B(7,7); // joint space inertia matrix in cartesian space
vpMatrix I7(7,7); // identity matrix
vpMatrix Ja_pinv_B_t(7,6); // pseudo-inverse of the Jacobian matrix multiplied by the inertia matrix
I7.eye();
{
robot.getPosition(vpRobot::JOINT_STATE, q);
std::cout<<"Initial joint position: "<<q.t()<<std::endl;
}
robot.setRobotState(vpRobot::STATE_FORCE_TORQUE_CONTROL);
robot.setCoppeliasimSyncMode(opt_settings.coppeliasim_sync_mode);
vpHomogeneousMatrix fMed, fMed0, fMe;
fMed = fMed0 = robot.get_fMe(); // get end-effector pose
vpMatrix K(6,6);
vpMatrix D(6,6);
vpMatrix edVf(6,6); // rotation matrix from operational frame to world frame(linear velocity and angular velocity)
double wp = 50.0; // position control gain
double wo = 20.0; // orientation control gain
K.diag({wp*wp, wp*wp, wp*wp, wo*wo, wo*wo, wo*wo});
D.diag({2*wp, 2*wp, 2*wp, 2*wo, 2*wo, 2*wo});
double mu = 0.1; // friction coefficient
std::cout<<"--Init finished. Press ctrl c to exit--"<<std::endl;
while(!symc::SignalManager::ShouldExit()){
robot.getPosition(vpRobot::JOINT_STATE, q);
robot.getVelocity(vpRobot::JOINT_STATE, q_dot);
robot.getMass(B);
robot.getCoriolis(C);
robot.getFriction(F);
robot.get_fJe(fJe); // get Jacobian matrix in operational frame
robot.getForceTorque(vpRobot::JOINT_STATE, tau);
fMed[1][3] = fMed0[1][3] + (sys_info.is_trajectory_started_ ? ( 0.1 * sin( 2 * M_PI * 0.3 * (sys_info.time_current_ - sys_info.time_start_trajectory_))) : 0 );
fMed[2][3] = fMed0[2][3] - (sys_info.is_trajectory_started_ ? ( 0.05 * sin( 2 * M_PI * 0.6 * (sys_info.time_current_ - sys_info.time_start_trajectory_))) : 0) ;
x_dot_desired[1] = (sys_info.is_trajectory_started_ ? (2 * M_PI * 0.3 * 0.1 * cos( 2 * M_PI * 0.3 * (sys_info.time_current_ - sys_info.time_start_trajectory_))) : 0) ;
x_dot_desired[2] = (sys_info.is_trajectory_started_ ? (-2 * M_PI * 0.6 * 0.05 * cos( 2 * M_PI * 0.6 * (sys_info.time_current_ - sys_info.time_start_trajectory_))) : 0) ;
x_dot_dot_desired[1] = (sys_info.is_trajectory_started_ ? (-2 * M_PI * 0.3 * 2 * M_PI * 0.3 * 0.1 * sin( 2 * M_PI * 0.3 * (sys_info.time_current_ - sys_info.time_start_trajectory_))) : 0) ;
x_dot_dot_desired[2] = (sys_info.is_trajectory_started_ ? (2 * M_PI * 0.6 * 2 * M_PI * 0.6 * 0.05 * sin( 2 * M_PI * 0.6 * (sys_info.time_current_ - sys_info.time_start_trajectory_))) : 0) ;
edVf.insert(fMed.getRotationMatrix().t(), 0, 0);
edVf.insert(fMed.getRotationMatrix().t(), 3, 3);
fMe = robot.get_fMe(); // get end-effector pose
vpPoseVector pose_error = static_cast<vpPoseVector>(fMed.inverse() * fMe); // get error in cartesian space
vpMatrix La = symc::Ta(pose_error); // compute Lie algebra
//TODO:La*edVf is the matrix that can transform the velocity in operational frame to the velocity in world frame?
Ja = La*edVf*fJe; // compute Jacobian matrix in world frame
if(!symc::IsEqual(sys_info.dt_, 0)){
Ja_dot = (Ja - Ja_old) / sys_info.dt_;
}else{
Ja_dot = 0;
}
Ja_old = Ja;
// weighted pseudo-inverse of the Jacobian matrix
// to optimize the control performance ?
// or to avoid singularity ?
// TODO:the formula is from the paper:
Ja_pinv_B_t = (Ja * B.inverseByCholesky()*Ja.t()).inverseByCholesky()*Ja*B.inverseByCholesky();
// compute desired joint torque
x_error = static_cast<vpColVector>(pose_error); // compute error in cartesian space
x_dot_error = La*edVf*(x_dot_desired - fJe*q_dot); // compute velocity error in cartesian space
// formula: tau = M(q)*q_dot_dot + C(q,q_dot) + F(q_dot) + G(q)
// Ja.pseudoInverse is the inverse of the Jacobian matrix
// which can transform cartesian space to joint space
vpMatrix control_law = (-K*(x_error)+D*(x_dot_error)-Ja_dot*q_dot + x_dot_dot_desired);
tau_desired = B*Ja.pseudoInverse()* control_law + C + F - (I7 - Ja.t()*Ja_pinv_B_t)*B*q_dot*100;
// tau_desired = B*Ja.pseudoInverse()* control_law + C + F;
if(sys_info.is_first_){
tau0 = tau_desired;
}
tau_cmd = tau_desired - tau0*std::exp(-mu*sys_info.time_current_ - sys_info.time_start_trajectory_);
// apply the desired joint torque
robot.setForceTorque(vpRobot::JOINT_STATE, tau_cmd);
if(opt_settings.verbose){
std::cout<<"dt: "<<sys_info.dt_<<std::endl;
}
robot.wait(sys_info.time_current_, 0.001); // Sync loop at 1000 Hz (1 ms)
sys_info.UpdateTime(robot);
}
symc::StopSimulation(robot);
}catch(const vpException& e){
std::cout<<"Catch an exception: "<<e.what()<<std::endl;
symc::StopSimulation(robot);
return EXIT_FAILURE;
}
std::cout<<"--Exited--"<<std::endl;
return EXIT_SUCCESS;
}
总结及推导
说明
这里直接跳过不太重要的部分，从字面意思即可理解其作用，重点说明while循环内的内容。
下面以base frame指代基坐标系（位姿），end-effector frame指代末端执行器坐标系（位姿），因为Homogeneous Transformation Matrix既可以表示变换也可以表示位姿，所以后面就不特意说明是位姿还是变换操作了。
用task space(cartesian space)指代任务空间（也就是笛卡尔空间），用joint space指代关节空间。
下面逐行进行解释。
初始化
1 | |
这里通过调用visp_ros的接口获取franka的信息，其中：
* getMass() 指获取质量矩阵(inertia matrix, 惯性矩阵)
* getCoriolis() 指获取科里奥利力矩阵
* getFriction() 指获取摩擦力矩阵
* get_fJe() 指获取雅可比矩阵(Jacobian
matrix)，注意这里是关节空间下的雅可比矩阵
其他根据字面意思理解即可。
1 | |
设定参考轨迹，也就是目标轨迹，fMed,
fMed0均是vpHomogeneousMatrix类型fMed[4][4]的index先是行再是列，与矩阵表示习惯相同。
这里贴一下vpHomogeneousMatrix类型的部分源码注释：
The class provides a data structure for the homogeneous matrices as well as a set of operations on these matrices.
The vpHomogeneousMatrix class is derived from vpArray2D<double>.
An homogeneous matrix is 4x4 matrix defines as \[\left[
^a{\bf M}_b = \left(\begin{array}{cc}
^a{\bf R}_b & ^a{\bf t}_b \\
{\bf 0}_{1\times 3} & 1
\end{array}
\right)
\right]\]
that defines the position of frame b in frame a
\(^a{R}_b\) is a rotation matrix and \(^a{t}_b\) is a translation vector.
矩阵更新
1 | |
TODO:这个没懂是干啥的，后面待补充。
1 | |
通过 get_fMe() 获取franka的姿态(Homogeneous transformation
matrix)，然后计算与目标位姿的误差，fMed是上面设置的目标位姿。
为什么 fMed.inverse() * fMe
得到的就是位姿误差？这里我的理解是fMed的形式应该是\(^{B}T_{D}\)(目标位姿相对基坐标系)，fMe的形式\({B}T_{E}\)(末端执行器位姿相对基坐标系)，通过左乘变换矩阵将fMe转换到目标坐标系下，即末端执行器相对目标坐标系的位姿表示，得到的即是位姿误差。
//TODO:这里理解可能是错误的
1 | |
通过 Ta
函数输入的齐次变换矩阵（代表末端执行器位姿），计算李代数，李代数是用来表示变换群（如SO(3)和SE(3)）的无穷小变换（即微分形式）。返回的是
6x6 的李代数矩阵，它表示了当前末端执行器位姿对应的微分变换。
它的作用是用于将6维的刚体运动（速度、力）从一个坐标系转换到另一个坐标系
然后计算雅可比矩阵（Ja），注意fJe是上面获取的关节空间下的雅可比矩阵，通过左乘La*edVf将其转换到笛卡尔空间下，得到笛卡尔空间下的雅可比矩阵Ja。
1
Ja_pinv_B_t = (Ja * B.inverseByCholesky()*Ja.t()).inverseByCholesky()*Ja*B.inverseByCholesky();
没看懂，根据chatgpt的说法，Ja_pinv_B_t是加权伪逆雅可比矩阵，其公式就是：
\[Ja\_pinv\_B\_t=(Ja \cdot {B^{−1}}\cdot
Ja^{T})^{-1}\cdot Ja\cdot B^{-1}\]
贴一下它的解释：
这个表达式 Ja_pinv_B_t 是计算 加权伪逆雅可比矩阵
的一种方式。加权伪逆（weighted
pseudoinverse）是用于解决欠定或超定系统中的伪逆运算，其中矩阵不一定是方阵，且需要考虑特定权重。
伪逆雅可比矩阵在控制和运动学中用于求解欠定或冗余系统的逆运动学问题。在这种情况下，加权伪逆
通过矩阵通过矩阵B引入了权重，使得解空间能够根据特定的权重进行优化。
加权伪逆的常见应用：
* 关节空间中的优化控制。
* 处理机器人冗余度时平衡关节的不同能量消耗或限制。
* 最小化在任务空间中实现运动时，关节空间中的力或速度。
该表达式通过Cholesky分解计算了加权伪逆雅可比矩阵。其目的是：
* 利用矩阵𝐵对雅可比矩阵进行加权，优化运动控制或冗余度处理。
* 使用Cholesky分解提升了计算效率，适用于正定对称矩阵𝐵的情境。
最终，Ja_pinv_B_t 是雅可比矩阵的加权伪逆，它在机器人控制和运动学求解中用于优化和解决欠定或冗余问题。
计算控制律
1 | |
在笛卡尔空间计算位置姿态误差和速度误差。
1
vpMatrix control_law = (-K*(x_error)+D*(x_dot_error)-Ja_dot*q_dot + x_dot_dot_desired);
K项为刚度控制项，对位置误差施加恢复力，类似于弹簧的作用，推动机器人回到期望位置。
D项阻尼控制项，利用速度误差来增加阻尼，抑制振荡和不稳定行为。
-Ja_dot*q_dot雅可比矩阵的变化影响项，考虑了机器人关节速度和雅可比矩阵随时间变化的耦合效应。
x_dot_dot_desired期望加速度，直接给出系统应该达到的加速度目标。
1
tau_desired = B*Ja.pseudoInverse()* control_law + C + F - (I7 - Ja.t()*Ja_pinv_B_t)*B*q_dot*100;
\[\tau = M(q)\cdot \ddot{q} + C(q,\dot{q}) +
F(\dot{q}) + G(q)\]
control_law左乘 Ja.pseudoInverse()
是将control_law从笛卡尔空间转换到任务空间。
后面的-(I7 -
Ja.t()*Ja_pinv_B_t)*B*q_dot*100根据chatgpt的解释是误差校正项，没看懂。
1 | |
计算真正的控制力矩tau_cmd，后面加上exp指数项是为了在启动时控制力矩是光滑的。