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

配置机械臂Ref: link注意如果只是仿真的话需要配置使用gazebo参数Pinocchiopythoncondainstallpinocchio-cconda-forgepythoncsudoaptinstall-qqylsb-releasecurlsudomkdir-p/etc/apt/keyringscurlhttp://robotpkg.openrobots.org/packages/debian/robotpkg.asc\|sudotee/etc/apt/keyrings/robotpkg.ascechodeb [archamd64 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*-pinocchioAdd env varible to ~/.bashrcexportPATH/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_PATHref: pinocchio installation确定关节位置接口这里关注controller配置:controllers_gazebocontroller_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数值优化逆解下图展示了数值优化逆解算法的迭代求解流程是否开始初始配置将目标位姿与初始关节角转换为 SE3 与 q迭代求解正向运动学计算当前 q 对应的末端位姿 oMi位姿误差计算dM target_pose⁻¹ · oMi对数映射err log(dM).vector收敛判断norm(err) eps ?收敛成功返回 True, qJacobian 计算J ∂oM/∂q更新关节配置v -Jᵀ(JJᵀ damp·I)⁻¹ errq integrate(q, v·DT)结束defgetIk(self,target_pos:np.ndarray,target_orientation:np.ndarray,joint_seed:np.ndarray,ik_weight:np.ndarrayNone):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_SE3pinocchio.SE3(pinocchio.Quaternion(target_orientation),target_pos)# q deepcopy(joint_seed).astype(np.float64)qnp.copy(joint_seed).astype(np.float64)foriinrange(self.IT_MAX):pinocchio.forwardKinematics(self.model,self.model_data,q)end_poseself.model_data.oMi[self.end_joint_id]error_posetarget_pose_SE3.actInv(end_pose)# T_sd*T_sb^(-1)errpinocchio.log(error_pose).vector# transform rotation to rotate vector by Rodrigues’ rotation formulaifnorm(err)self.eps:qself.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 Jpinocchio.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-MarquardtLMv_jJ.T.dot(v_e)# map to joint spaceqpinocchio.integrate(self.model,q,v_j*self.DT)# delta q_k1 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)errlog(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(JJTdamp⋅I)−1err用( v ) ( v )(v)和时间步长( D T ) ( DT )(DT)更新关节位置q integrate ( q , v ⋅ D T ) q \text{integrate}(q, v \cdot DT)qintegrate(q,v⋅DT)这里可以参考现代机器人学第六章数值求解部分, 不过这里的数值优化方法采用的是 LM方法Levenberg-Marquardt 最小二乘优化运行roslaunch rm_gazebo arm_75_bringup_moveit.launchsourcedevel/setup.bash# or setup.zshrosrun inv_pino.py开源代码在这里如果有帮助的话,给个星吧Refhttps://blog.csdn.net/weixin_39284111/article/details/141307019https://zhuanlan.zhihu.com/p/42415718https://blog.csdn.net/huangjunsheng123/article/details/108393690