rm-controls/rm_control

全向轮不能左右运动

Closed this issue · 8 comments

前后运动和自旋都正常,但是cmd_vel发送y方向的速度,不管发送多少速度,set_point都是0
已测试过麦轮的运动,都正常,只有全向轮会出现这个问题,controller的配置文件都正确
试了一下加载rm_description中的其他机器人urdf模型,hero,standard3,standard5都试过,都是一样的问题

麦轮和全向轮虽然用的是同一个controller,但是配置文件有区别。
全向轮配置
wheels: left_front: pose: [ 0.147, 0.147, 2.356 ] joint: left_front_wheel_joint <<: &wheel_setting roller_angle: 0. radius: 0.07625 pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } right_front: pose: [ 0.147, -0.147, 0.785 ] joint: right_front_wheel_joint <<: *wheel_setting left_back: pose: [ -0.147, 0.147, -2.356 ] joint: left_back_wheel_joint <<: *wheel_setting right_back: pose: [ -0.147, -0.147, -0.785 ] joint: right_back_wheel_joint <<: *wheel_setting
麦轮配置
wheels: left_front: pose: [ 0.219, 0.215, 0. ] roller_angle: -0.785 joint: left_front_wheel_joint <<: &wheel_setting radius: 0.07625 pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } right_front: pose: [ 0.219, -0.215, 0. ] roller_angle: 0.785 joint: right_front_wheel_joint <<: *wheel_setting left_back: pose: [ -0.219, 0.215, 0. ] roller_angle: 0.785 joint: left_back_wheel_joint <<: *wheel_setting right_back: pose: [ -0.219, -0.215, 0. ] roller_angle: -0.785 joint: right_back_wheel_joint <<: *wheel_setting

pose的前两个数是底盘坐标系下轮子的坐标x,y,部署时需要根据实车调整。

发现问题:配置文件没有错,但是电机到关节状态的映射错了,如图,此时我发了角速度z,全向轮应该自旋,4个关节状态和电机的速度都应该同号,但是执行器电机有两个不同号的,导致此时本应该自旋,全向轮却往x轴正方向走了
猜测是关节状态的映射依然还是麦轮的映射,于是我在rm_hw文件里load分别了standard5和standard3的urdf,我看了rm_description里是全向轮的关节映射,但是最后的结果依然不行
205_1700821107_hd

发现问题:配置文件没有错,但是电机到关节状态的映射错了,如图,此时我发了角速度z,全向轮应该自旋,4个关节状态和电机的速度都应该同号,但是执行器电机有两个不同号的,导致此时本应该自旋,全向轮却往x轴正方向走了 猜测是关节状态的映射依然还是麦轮的映射,于是我在rm_hw文件里load分别了standard5和standard3的urdf,我看了rm_description里是全向轮的关节映射,但是最后的结果依然不行 205_1700821107_hd

你在跑仿真还是实车?
检查对关节的速度力矩指令是不是对的,如果是对的,关节和电机的映射是在urdf里transmission这个标签描述的,把对应的减速比正负号取反,再检查映射对了没有

你好,我在实车上测试的,已经解决,但是很奇怪,我把rm_description包放在本地编译后问题就解决了,之前是从源添加的就有这个问题

佬,在实车测试的时候还有一个问题,车已经动了,但是在rviz里的base_link没有移动,只能在原点自旋时会动
2023-11-25_13-16

发现问题了,我没有订阅到/odometry,我看你们用的惯性里程,在实际比赛的时候飘得厉害吗

发现问题了,我没有订阅到/odometry,我看你们用的惯性里程,在实际比赛的时候飘得厉害吗

轮式里程计只是调试的时候看一看,实际上并不会用到。哨兵是用雷达做里程计的

好的!没有问题了!已经可以建图,这个框架真好用!