ROS 机械臂通过Pinocchio逆解直接控制关节位置仿真-三亩地-编程学习日记

配置

机械臂

Ref: link
注意如果只是仿真的话需要配置使用gazebo参数

Pinocchio

python

condainstallpinocchio-cconda-forge

python&&c++

sudoaptinstall-qqylsb-releasecurlsudomkdir-p/etc/apt/keyringscurlhttp://robotpkg.openrobots.org/packages/debian/robotpkg.asc\|sudotee/etc/apt/keyrings/robotpkg.ascecho"deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub$(lsb_release-cs)robotpkg"\|sudotee/etc/apt/sources.list.d/robotpkg.listsudoaptupdatesudoaptinstall-qqyrobotpkg-py3*-pinocchio

Add env varible to ~/.bashrc

exportPATH=/opt/openrobots/bin:$PATHexportPKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATHexportLD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATHexportPYTHONPATH=/opt/openrobots/lib/python3.8/site-packages:$PYTHONPATH# Adapt your desired python version hereexportCMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH

ref: pinocchio installation

确定关节位置接口

这里关注controller配置:
controllers_gazebo

controller_manager_ns:controller_managercontroller_list:-name:arm/arm_joint_controlleraction_ns:follow_joint_trajectorytype:FollowJointTrajectorydefault:truejoints:-joint1-joint2-joint3-joint4-joint5-joint6-joint7

那么可以确定动作接口为arm/arm_joint_controller/follow_joint_trajectory
对应代码中的
self.client = actionlib.SimpleActionClient('/arm/arm_joint_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
这里还和大家说一声, 有些关节控制采用类似如下形式:

rrbot:# Publish all joint states -----------------------------------joint_state_controller:type:joint_state_controller/JointStateControllerpublish_rate:50# Position Controllers ---------------------------------------joint1_position_controller:type:effort_controllers/JointPositionControllerjoint:joint1pid:{p:100.0,i:0.01,d:10.0}joint2_position_controller:type:effort_controllers/JointPositionControllerjoint:joint2pid:{p:100.0,i:0.01,d:10.0}

那么在控制接口上需要进行修改
可以直接话题控制,参考
https://blog.csdn.net/huangjunsheng123/article/details/108393690

数值优化逆解

下图展示了数值优化逆解算法的迭代求解流程：

defgetIk(self,target_pos:np.ndarray,target_orientation:np.ndarray,joint_seed:np.ndarray,ik_weight:np.ndarray=None):"""Computes the inverse kinematics for a given target pose."""ifnotisinstance(target_pos,np.ndarray)\ortarget_pos.shape!=(3,):raiseValueError("target_pose must be a 1x3 numpy array")ifnotisinstance(target_orientation,np.ndarray)\ortarget_orientation.shape!=(4,):raiseValueError("target_orientation must be a 1x4 numpy array")ifnotisinstance(joint_seed,np.ndarray):raiseValueError("joint_seed must be of type np.ndarray")target_pose_SE3=pinocchio.SE3(pinocchio.Quaternion(target_orientation),target_pos)# q = deepcopy(joint_seed).astype(np.float64)q=np.copy(joint_seed).astype(np.float64)foriinrange(self.IT_MAX):pinocchio.forwardKinematics(self.model,self.model_data,q)end_pose=self.model_data.oMi[self.end_joint_id]error_pose=target_pose_SE3.actInv(end_pose)# T_sd*T_sb^(-1)err=pinocchio.log(error_pose).vector# transform rotation to rotate vector by Rodrigues’ rotation formulaifnorm(err)<self.eps:q=self.qpos_to_limits(q,self.model.upperPositionLimit,self.model.lowerPositionLimit,joint_seed,ik_weight)ifself.is_log:print("Convergence iteration ")print("Pin:{} error = {}!".format(i,err.T))self.getFk(q)returnTrue,q J=pinocchio.computeJointJacobian(self.model,self.model_data,q,self.end_joint_id)v_e=-solve(J.dot(J.T)+self.damp*np.eye(6),err)#Levenberg-Marquardt（LM）v_j=J.T.dot(v_e)# map to joint spaceq=pinocchio.integrate(self.model,q,v_j*self.DT)# delta q_k+1 =q_k+ \delta qifnoti%10andself.is_log:print("Pin:{} error = {}!".format(i,err.T))print("Pin:The iterative algorithm has not reached convergence to the desired precision")returnFalse,q

初始配置：
将初始位姿转换se3,为什么要做这一步呢？这是希望将误差变换矩阵映射到李代数空间，而李代数提供可微的旋转误差表示。注意这个向量是6维的，位置差和3维轴角（2个轴方向参数+1个角度参数）描述的旋转误差。
迭代求解:
- 正向运动学: 计算当前关节配置 ( q ) 的正向运动学，得到末端执行器的位置和方向。
- 位姿误差计算:
  d M i = target_pose − 1 ⋅ o M i dM_i = \text{target\_pose}^{-1} \cdot oMidMi=target_pose−1⋅oMi
  其中 ( oMi ) 是当前配置的末端位姿，( dM_i ) 是位姿的误差。
- 对数映射: 计算 ( dM_i ) 的对数映射，得到位置误差：
  e r r = log ( d M i ) err = \text{log}(dM_i)err=log(dMi)，
收敛判断: 如果error误差小于阈值eps，认为已经收敛。
Jacobian 和速度计算:
- Jacobian计算:
  J = ∂ o M ∂ q （Jacobian matrix） J = \frac{\partial oM}{\partial q} \quad \text{（Jacobian matrix）}J=∂q∂oM（Jacobian matrix）
- 更新关节配置:
  v = − J T ( J J T + damp ⋅ I ) − 1 e r r v = - J^T (J J^T + \text{damp} \cdot I)^{-1} errv=−JT(JJT+damp⋅I)−1err
- 用( v ) ( v )(v)和时间步长( D T ) ( DT )(DT)更新关节位置：
  q = integrate ( q , v ⋅ D T ) q = \text{integrate}(q, v \cdot DT)q=integrate(q,v⋅DT)

这里可以参考现代机器人学第六章数值求解部分, 不过这里的数值优化方法采用的是 LM方法

Levenberg-Marquardt 最小二乘优化

运行

roslaunch rm_gazebo arm_75_bringup_moveit.launch

sourcedevel/setup.bash# or setup.zshrosrun inv_pino.py

开源代码在这里
如果有帮助的话,给个星吧

Ref

https://blog.csdn.net/weixin_39284111/article/details/141307019
https://zhuanlan.zhihu.com/p/42415718
https://blog.csdn.net/huangjunsheng123/article/details/108393690

日记详情

ROS 机械臂通过Pinocchio逆解直接控制关节位置仿真

配置

机械臂

Pinocchio

确定关节位置接口

数值优化逆解

运行

Ref