shuoyangrobotics/a1-qp-mpc-controller

An open source implementation of MIT Cheetah 3 controllers

a1-qp-mpc-controller's Issues

Unexpected behavior on hardware

Thanks for releasing this code!

I have successfully compiled (without docker) on the A1, and ran a separate script which sent a command of all 0's, to enter "standby" mode as mentioned in the readme.

After running: roslaunch a1_cpp a1_ctrl.launch type:=hardware solver_type:=mpc

the robot jumps slightly in the air, and then is completely unresponsive. It's unclear to me why this behavior is occurring, and if anyone else has experienced this or has advice in debugging the issue, it would be appreciated!

lacking header file "auxil.h" issue solve

Since the osqp and osqp-eigen has version dismatching problem, you may have issue when catkin make on controller.
refer to this link to solve :

https://openatomworkshop.csdn.net/664eef17b12a9d168eb72c04.html?dp_token=eyJ0eXAiOiJKV1QiLCJhbGciOiJIUzI1NiJ9.eyJpZCI6NjI0NDczOCwiZXhwIjoxNzE5MjA1ODc3LCJpYXQiOjE3MTg2MDEwNzcsInVzZXJuYW1lIjoiZmdnamhiaCJ9.WD1EbNM37mwekaYRYdcKIqhqOSErJwXAY7CUbbqVDcE

Revised MPC Controller

Hello,

I would like to implement this MPC controller on an A1, but the link for the revised MPC controller is developed at CMU REX Lab Github says Error 404 Not Found. Is there a new link to this repository? Also was there a paper where the formulation and theory for this MPC controller is outlined? I would also like to take a look at that and deploy the MPC on an A1, thanks!

Best,

Kevin

problem about trotting up stairs in gazebo

Hi, I tried your code in gazebo, running in the docker environment. The simulation step is small and the locomotion does not match with real-time performance. Also, the parameters of ODE engine affect largely the locomotion. It seems that the dog never falls down from the stairs even when its head is in touch with the stairs, which is unreasonable in reality.

Major problems in robot param & MPC control

in gazebo_a1_mpc.yaml the a1_trunk_inertia is set to
a1_trunk_inertia_xx: 0.0158533 a1_trunk_inertia_xy: 0.0 a1_trunk_inertia_xz: 0.0 a1_trunk_inertia_yz: 0.0 a1_trunk_inertia_yy: 0.0377999 a1_trunk_inertia_zz: 0.0456542
which is obviously copied from the urdf file of a1 robot description.
However, such inertia param is only the trunk param. According to the urdf robot description, the main inertia of the robot is composed by the mass of leg motors, which contribute to more than 50% percent of the robot mass, and locate at farther postions from the robot COM.
Therefore, the inertia param above needs carefully recalculating if the single-rigid-body model is used. I found that if they are multipled by 10 or 20, the angular prediction in MPC will work much better. (I havn't fine-tuned them)

==============
Besides, the prediction horizon of MPC also has major problems.
In file A1RobotControl.cpp, the mpc_dt is set to 2.5 ms, and the prediction step is set to 10. Therefore the prediction horizon is only 25 ms, which is way smaller than the description given by the original paper, and is unable to make resonable predctions.

The proposed controller was implemented on the MIT Cheetah 3 Robot [26] with a horizon length of one gait cycle (0.33 to 0.5 seconds), subdivided into between 10 and 16 timesteps, depending on the gait selected.

In fact, the time interval for mpc prediction does not have to be idential to the program update interval. In my practice, it can be set to 0.05 sec, which results in a 0.5 second prediction horizon. Such horizon allows better prediction performance and reduces noises.

I also notice that the A and B matrix in MPC control is not predicted but constant during every program update (constant root_rot_mat). The upper and lower bound of mpc is also constant instead of predicting future contacts. Maybe it will improve the performance of the MPC controller if such prediction is taken into account, just as in the original paper.

Thanks again for your contribution to the community. It helps me a lot. : )

Error in Gazebo example

When running the Gazebo example, I got the following error in a1_cpp_ctrl_image. The build had no problems.

root@URANUS:~/A1_ctrl_ws# roslaunch a1_cpp a1_ctrl.launch type:=gazebo solver_type:=mpc
... logging to /root/.ros/log/d7c301c8-37e7-11ed-ae13-5800e38f14b5/roslaunch-URANUS-692.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://localhost:46479/

SUMMARY
========

PARAMETERS
 * /a1_default_foot_pos_FL_x: 0.17
 * /a1_default_foot_pos_FL_y: 0.15
 * /a1_default_foot_pos_FL_z: -0.35
 * /a1_default_foot_pos_FR_x: 0.17
 * /a1_default_foot_pos_FR_y: -0.15
 * /a1_default_foot_pos_FR_z: -0.35
 * /a1_default_foot_pos_RL_x: -0.17
 * /a1_default_foot_pos_RL_y: 0.15
 * /a1_default_foot_pos_RL_z: -0.35
 * /a1_default_foot_pos_RR_x: -0.17
 * /a1_default_foot_pos_RR_y: -0.15
 * /a1_default_foot_pos_RR_z: -0.35
 * /a1_gait_counter_speed_FL: 1.5
 * /a1_gait_counter_speed_FR: 1.5
 * /a1_gait_counter_speed_RL: 1.5
 * /a1_gait_counter_speed_RR: 1.5
 * /a1_kd_foot_x: 10.0
 * /a1_kd_foot_y: 10.0
 * /a1_kd_foot_z: 5.0
 * /a1_km_foot_x: 0.1
 * /a1_km_foot_y: 0.1
 * /a1_km_foot_z: 0.1
 * /a1_kp_foot_x: 200.0
 * /a1_kp_foot_y: 200.0
 * /a1_kp_foot_z: 150.0
 * /a1_robot_mass: 12.0
 * /a1_trunk_inertia_xx: 0.0158533
 * /a1_trunk_inertia_xy: 0.0
 * /a1_trunk_inertia_xz: 0.0
 * /a1_trunk_inertia_yy: 0.0377999
 * /a1_trunk_inertia_yz: 0.0
 * /a1_trunk_inertia_zz: 0.0456542
 * /q_weights_0: 20.0
 * /q_weights_10: 30.0
 * /q_weights_11: 10.0
 * /q_weights_12: 0.0
 * /q_weights_1: 10.0
 * /q_weights_2: 1.0
 * /q_weights_3: 0.0
 * /q_weights_4: 0.0
 * /q_weights_5: 420.0
 * /q_weights_6: 0.05
 * /q_weights_7: 0.05
 * /q_weights_8: 0.05
 * /q_weights_9: 30.0
 * /r_weights_0: 1e-7
 * /r_weights_10: 1e-7
 * /r_weights_11: 1e-7
 * /r_weights_1: 1e-7
 * /r_weights_2: 1e-7
 * /r_weights_3: 1e-7
 * /r_weights_4: 1e-7
 * /r_weights_5: 1e-7
 * /r_weights_6: 1e-7
 * /r_weights_7: 1e-7
 * /r_weights_8: 1e-7
 * /r_weights_9: 1e-7
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /stance_leg_control_type: 1
 * /use_sim_time: True

NODES
  /
    gazebo_a1_ctrl (a1_cpp/gazebo_a1_ctrl)

ROS_MASTER_URI=http://localhost:11311

process[gazebo_a1_ctrl-1]: started with pid [701]
init A1RobotControl
init A1RobotControl
init nh
Enter thread 1: compute desired ground forces
Enter thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands
foot_pos_recent_contact z: 0 0 0 0
[DEBUG] [1663570540.605644433, 24.637500000]: Trying to publish message of type [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7] on a publisher with type [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7]
[DEBUG] [1663570540.606794327, 24.637600000]: Trying to publish message of type [std_msgs/Float64/fdb28210bfa9d7c91146260178d9a584] on a publisher with type [std_msgs/Float64/fdb28210bfa9d7c91146260178d9a584]
desire pitch in deg: 0
terrain angle: 0
[DEBUG] [1663570540.606865943, 24.637600000]: Trying to publish message of type [unitree_legged_msgs/MotorCmd/bbb3b7d91319c3a1b99055f0149ba221] on a publisher with type [unitree_legged_msgs/MotorCmd/bbb3b7d91319c3a1b99055f0149ba221]
ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex
ERROR in osqp_setup: KKT matrix factorization.
The problem seems to be non-convex.
[OsqpEigen::Solver::initSolver] Unable to setup the workspace.
[OsqpEigen::Solver::solveProblem] The solve has not been initialized yet. Please call initSolver() method.
[OsqpEigen::Solver::solve] Unable to solve the problem.
[gazebo_a1_ctrl-1] process has died [pid 701, exit code -11, cmd /root/A1_ctrl_ws/devel/lib/a1_cpp/gazebo_a1_ctrl __name:=gazebo_a1_ctrl __log:=/root/.ros/log/d7c301c8-37e7-11ed-ae13-5800e38f14b5/gazebo_a1_ctrl-1.log].
log file: /root/.ros/log/d7c301c8-37e7-11ed-ae13-5800e38f14b5/gazebo_a1_ctrl-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Do you have any idea what is causing this error?

Will this pkg be used in Go1 directly?

Hi, will this pkg can be used in Unitree Go1 robot now?
and I'm not sure the contact detection can be change (I also found a pkg from Aaron lab: https://github.com/robomechanics/quad-sdk,
and I notice you also mentioned contact point in your newly pkg "legged-kinematics-calibration")

PATH_OF_THE_REPO_ON_YOUR_HOST_COMPUTER

I really don't know what is PATH_OF_THE_REPO_ON_YOUR_HOST_COMPUTER ?
Any example ? I'm running with Ubuntu 20.04

docker run -d --network host --cap-add=IPC_LOCK --cap-add=sys_nice -v /home/gigijoes/dog/A1-QP-MPC-Controller/docker:/root/A1_ctrl_ws/src/A1_Ctrl --device /dev/input --name a1_cpp_ctrl_docker a1_cpp_ctrl_image
docker: Error response from daemon: Conflict. The container name "/a1_cpp_ctrl_docker" is already in use by container "044e3ea292d7ecdf173cdc3448b38876b2c68bc177c4ace14aba821b873e77b8". You have to remove (or rename) that container to be able to reuse that name.
See 'docker run --help'.

Why are the parameter "a1_robot_mass" different in config folders?

Hello, i found that the parameter "a1_robot_mass" is different in the "config" folder's yaml files. In "gazebo_a1_mpc.yaml" and "gazebo_a1_mpc.yaml"(line 6), the value is 12.0 :

# physics parameters

a1_robot_mass: 12.0 

but in "hardware_a1_mpc.yaml" and "hardware_a1_mpc.yaml", the value has changed to 13.5 and 15.0

# physics parameters(qp)

a1_robot_mass: 15.0
————————————————————————————————————————
# physics parameters(mpc)

a1_robot_mass: 13.5

i want to know why the mass is different in two methods. Besides, i also found that the parameter "a1_trunk_inertia_xx" is also different in in "hardware_a1_mpc.yaml" and in "gazebo_a1_mpc.yaml".

Looking forward to your reply.

Error catkin_make

Hi, I'm trying to make this repo work without Docker bc my team is not comfortable with it.
But even after installing osqp-eigen (from source and with conda) I obtain this error:

In file included from /usr/include/OsqpEigen/OsqpEigen.h:10,
                 from /home/parallels/capstone/workspace/src/a1_cpp/src/test/test_rotation.cpp:8:
/usr/include/OsqpEigen/Constants.hpp:12:10: fatal error: osqp.h: No such file or directory
   12 | #include <osqp.h>
      |          ^~~~~~~~
compilation terminated.
make[2]: *** [a1_cpp/CMakeFiles/test_rotation.dir/build.make:63: a1_cpp/CMakeFiles/test_rotation.dir/src/test/test_rotation.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:2868: a1_cpp/CMakeFiles/test_rotation.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
Invoking "make -j4 -l4" failed

How do you guys have installed it ?

Why using inverse kinematics instead of the jacobian transpose in swing leg PD control?

RT.
In swing leg control, the code uses the inverse kinematics for torque calculation
Eigen::Vector3d force_tgt = state.km_foot.cwiseProduct(state.foot_forces_kin.block<3, 1>(0, i)); joint_torques.segment<3>(i * 3) = jac.lu().solve(force_tgt);
While in the original paper

Bledt, G., Powell, M. J., Katz, B., Di Carlo, J., Wensing, P. M., & Kim, S. (2018). MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot. IEEE International Conference on Intelligent Robots and Systems, 2245–2252. https://doi.org/10.1109/IROS.2018.8593885

the transpose of jacobian is used to calculate the PD control torque in Eq.(8).
What is the difference between these two approaches? Feel free to use Chinese.

why 4th order Bezier Curves is used instead of 3rd order, like MIT?

In the code I notice that 4th order Bezier Curves with 5 control point is used. I am curios about the reason why it is adopted. Thank you!

关于仿真和实物调试的问题

你好，我们首先按照README的方法在gazebo中运行。当执行完rosrun unitree_controller unitree_servo和rosrun unitree_controller unitree_move_kinetic这两条命令之后机器人四条腿处于支撑状态并回到原点位置，再运行控制器roslaunch a1_cpp a1_ctrl.launch type:=gazebo solver_type:=mpc，机器人站立状态就会变得不稳定。我们通过XBOX手柄按A键，让机器人从站立变为行走模式，再按下A键之后，机器人变为站立模式，这个时候机器人的腿会出现抖动的情况。README里提到不同的电脑可能会有不同的性能，我们使用的电脑的配置的CPU是r7-4800h，显卡是2060的，会有可能是电脑性能不够的原因吗？我们的仿真中机器人处于行走模式的时候，和演示的视频效果是差不多的，但就是在启动阶段以及从行走模式切换回站立模式的时候会出现不稳定状态。演示视频中只有机器人启动之后的演示效果，不知道启动阶段是否也会存在这样的问题呢？

我们也尝试将这套代码移植到宇树的Go1上，根据之前提的issue的说法，我们在安装的时候使用的是宇树最新的unitree_legged_sdk，将config文件中新增了一个hardware_go1_mpc.yaml，将机器人的质量、身体的转动惯量设置为Go1的参数，也将HardwareA1ROS.cpp中A1的腿的偏置、电机的偏置以及腿长等参数修改成了Go1的参数，并修改a1_ctrl.launch文件。我们在调试的时候，让机器人开机并站立起来后，按遥控器的L2+B让机器人进入阻尼模进并趴下，然后在docker中运行控制器，通过网线直接连接机器人。机器人会突然动一下然后无法站立并且直接进入保护模式，我们将传给电机的力矩信息输出来发现会达到35N·m。但如果我们在机器人开机并站立之后直接运行控制器，机器人就会像仿真中一样出现抖动的状态，我们觉得这可能是由于电机高频电流造成的振荡，所以尝试着将hardware_go1_mpc.yaml中的a1_kd_foot_x，a1_kd_foot_y，a1_kd_foot_z调小，但是却没有起作用。我想问一下，README中的standby模式是指阻尼模式吗？我不太确定我们运行的方法是否是正确的。还有就是除了我们修改的部分，还有什么是需要调整的吗？

希望得到你的回复。

where is the wname and world file about the picture.,thanks

where is the wname and world file about the picture.,thanks

Init-Compilation Failure Issue

Hello, I recently started playing your code, but I encountered some issues during the initial compilation. In the following, it shows that the 'auxil.h' header file does not exist, and the error message is:

Errors << a1_cpp:make /root/A1_ctrl_ws/logs/a1_cpp/build.make.049.log
In file included from /usr/local/include/OsqpEigen/Solver.hpp:312:0,
from /usr/local/include/OsqpEigen/OsqpEigen.h:13,
from /root/A1_ctrl_ws/src/A1_Ctrl/A1-QP-MPC-Controller/src/a1_cpp/src/test/test_rotation.cpp:8:
/usr/local/include/OsqpEigen/Solver.tpp:10:10: fatal error: auxil.h: No such file or directory
#include <auxil.h>
^~~~~~~~~

I have tried some other methods（adding paths to the header files ; find the auxil.h path: /tmp/osqp/include/private and add to the include_directories in cmakelists） to resolve this issue, but each time I encounter similar errors indicating that other header files are missing or just completely wrong. I also found that the auxil.h exits in some different path, which seems like a little bit strange here. I'm still relatively new to this, so I'm hoping to get your response. Thanks a lot!!

在移植a1qpmpc时遇到一些问题

1.前向运动学计算足端位置或许位置不对，运动学和动力学的输入关节状态是该关节的哪个角度和角速度，具体正负方向如何
2.或许由于足端位置不对，所计算的地面反作用力也不对，导致关节驱动力矩计算错误，也有可能是力矩方向问题？（在用pd控制器进行位控时可以观测到关节力矩，同时与控制器计算力矩比较发现错误）
3.motor_offset修正值是如何计算得到的？
4.p_br和R_br是实测的还是计算得到的（imu2body转化吗？）

localhost port 2233: Connection refused

When I try to ssh root@localhost -p2233 I obtain ssh: connect to host localhost port 2233: Connection refused.

Here is what I've done:

1. Changed the CMakefile in unitree_legged_sdk to use my arm64 architecture
2. Remove everything everything related to Aliengo in Dockerfile because not compatible with amd64
3. Remove tensorflow from Dockerfile

Can anyone help me ?

有在AMD显卡上成功运行的吗？

有在AMD显卡上成功运行的吗？

Is there any way to get body accelerations from Isaac Sim?

I notice in IsaacA1ROS.cpp, IMU acceleration data is provided by isaac sim, but I cannot find any API in isaac gym to fetch body acceleration data, is there any other way to get the data?

梁亭演示

Then we create a script to run the image called run_gazebo_docker.bash 这个怎样运行？请大佬们指点一下！我刚接触docker

Misleading information for using joystick in docker

In the readme,
"If there is a USB joystick connects to the host computer. The docker should be able to see it (because when we run the docker container, we use --device /dev/input to map USB input devices into the docker). So we can read its data in the docker. Open another terminal

ssh root@localhost -p2233
rosrun joy joy_node "

In my case, I cannot connect the joystick by running the command in docker. Instead, I run the command
rosrun joy joy_node
in the host pc, then the joystick can be found in the docker.

Someone else may encounter the same issue, it will be helpful if you can add some notes to remind them.

相关问题

1.摆动腿的的力矩控制在原始论文中有前馈项，似乎代码里面没有体现；A1RobotControl.cpp中第310行的注释// jac * tau = F是什么意思？jac似乎不是对称阵，这个是否满足？
2. GazeboA1ROS.cpp中第150-161行，从手柄获取的速度与位置指令（包括root_ang_vel_d、root_euler_d、root_pos_d）分别是表示在世界坐标系还是在基座坐标系中的呢？而相应的速度与位置状态量（不加'_d')又表示在哪个坐标系中呢？代码中a1_ctrl_states.root_ang_vel_d[0] = joy_cmd_roll_rate; 及后续赋值语句中，角速度与横滚rate、俯仰rate、偏航rate并不相等，为什么直接赋值呢？似乎后来对root_ang_vel_d在状态中也是当作角速度项处理的。
3. A1RobotControl.cpp第501-504行，论文中的MPC预测时域中的B_mat_c是随着未来的偏航角与四个落足位置变化的，代码中的B_mat_c是否有变化？

What is the parameter ‘km_foot’?

Find that the parameter km_foot in code
Eigen::Vector3d force_tgt = state.km_foot.cwiseProduct(state.foot_forces_kin.block<3, 1>(0, i));
Where
km_foot = Eigen::Vector3d(0.1, 0.1, 0.1);
What does this parameter represent？Why not adjusting parameter kp_foot and kd_foot instead?

ERR when execute docker build -t a1_cpp_ctrl_image .

Step 56/81 : RUN git clone https://github.com/unitreerobotics/aliengo_sdk.git && cd ${SUPPORT_WS}/aliengo_sdk && mkdir build && cd build && cmake .. && make -j12
---> Using cache
---> 7f0dc61c9f78
Step 57/81 : RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi &amp;&amp; apt-get update &amp;&amp; apt-get install -y ros-${ROS_DISTRO}-ros-control ros-${ROS_DISTRO}-gazebo-ros ros-${ROS_DISTRO}-joy ros-${ROS_DISTRO}-ros-controllers ros-${ROS_DISTRO}-robot-state-publisher
---> Running in 20366730b24e
Err:1 http://security.ubuntu.com/ubuntu bionic-security InRelease
Connection failed [IP: 91.189.91.38 80]
Err:2 http://archive.ubuntu.com/ubuntu bionic InRelease
Connection failed [IP: 91.189.91.38 80]
Hit:3 http://packages.ros.org/ros/ubuntu bionic InRelease
Err:4 http://archive.ubuntu.com/ubuntu bionic-updates InRelease
Connection failed [IP: 91.189.91.39 80]
Err:5 http://archive.ubuntu.com/ubuntu bionic-backports InRelease
Connection failed [IP: 91.189.91.38 80]
Reading package lists...
W: Target Packages (main/binary-amd64/Packages) is configured multiple times in /etc/apt/sources.list.d/ros-latest.list:1 and /etc/apt/sources.list.d/ros1-latest.list:1
W: Target Packages (main/binary-all/Packages) is configured multiple times in /etc/apt/sources.list.d/ros-latest.list:1 and /etc/apt/sources.list.d/ros1-latest.list:1
W: Failed to fetch http://archive.ubuntu.com/ubuntu/dists/bionic/InRelease Connection failed [IP: 91.189.91.38 80]
W: Failed to fetch http://archive.ubuntu.com/ubuntu/dists/bionic-updates/InRelease Connection failed [IP: 91.189.91.39 80]
W: Failed to fetch http://archive.ubuntu.com/ubuntu/dists/bionic-backports/InRelease Connection failed [IP: 91.189.91.38 80]
W: Failed to fetch http://security.ubuntu.com/ubuntu/dists/bionic-security/InRelease Connection failed [IP: 91.189.91.38 80]
W: Some index files failed to download. They have been ignored, or old ones used instead.
W: Target Packages (main/binary-amd64/Packages) is configured multiple times in /etc/apt/sources.list.d/ros-latest.list:1 and /etc/apt/sources.list.d/ros1-latest.list:1
W: Target Packages (main/binary-all/Packages) is configured multiple times in /etc/apt/sources.list.d/ros-latest.list:1 and /etc/apt/sources.list.d/ros1-latest.list:1
Reading package lists...
Building dependency tree...

E: Failed to fetch http://security.ubuntu.com/ubuntu/pool/main/c/curl/libcurl4-openssl-dev_7.58.0-2ubuntu3.17_amd64.deb Connection failed [IP: 91.189.91.38 80]
E: Failed to fetch http://archive.ubuntu.com/ubuntu/pool/main/f/fonts-lato/fonts-lato_2.0-2_all.deb Connection failed [IP: 91.189.91.39 80]
E: Failed to fetch http://security.ubuntu.com/ubuntu/pool/main/q/qtsvg-opensource-src/libqt5svg5_5.9.5-0ubuntu1.1_amd64.deb Connection failed [IP: 91.189.91.38 80]
E: Failed to fetch http://archive.ubuntu.com/ubuntu/pool/universe/libc/libccd/libccd2_2.0-1_amd64.deb Connection failed [IP: 91.189.91.39 80]
E: Failed to fetch http://security.ubuntu.com/ubuntu/pool/main/a/apparmor/libapparmor1_2.12-4ubuntu5.1_amd64.deb Connection failed [IP: 91.189.91.38 80]
E: Failed to fetch http://security.ubuntu.com/ubuntu/pool/main/f/fribidi/libfribidi0_0.19.7-2ubuntu0.1_amd64.deb Connection failed [IP: 91.189.91.39 80]
E: Failed to fetch http://archive.ubuntu.com/ubuntu/pool/main/s/slang2/libslang2_2.3.1a-3ubuntu1_amd64.deb Connection failed [IP: 91.189.91.38 80]

Possible missing coefficient when converting QP controller to osqp.

in https://github.com/ShuoYangRobotics/A1-QP-MPC-Controller/blob/main/src/a1_cpp/src/A1RobotControl.cpp,A1RobotControl.cpp, line404, the gredient of osqp is calculated by
gradient.block<3 * NUM_LEG, 1>(0, 0) = -inertia_inv.transpose() * Q * root_acc;
It seems that a coefficient 2 is missing, which may cause undesirable performance.

Controller does not continue/no topics from gazebo received

Hello,

I launch the controller, but it does not continue after "Enter thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands". I can image because it does not receive any messages. I run gazebo in the other container. I tried to run gazebo in the same container but i canot launch it altough of course i sourced unitree_ws. I also dont manage to build unitree_ws.
Can you help me?
I work on Ubunutu20.04

Thanks

does your controller need foot sensors ? Or can I do without them ?!

Hello, thanks for your very nice package.
I'd like to test it on my own real robot.
My robot uses bldc motors, but don't have foot sensors.
Is your package compatible with my robot ?
Well your package doesn't need foot sensors ( or perhaps you have a sensorless version ?)
Impatient to read you.
Best
Vincent

static error with QP control when trying to stand still

I'm trying to use the code of QP control to let the robot stand up and stay still. However, no matter how I tune the parameter of the controoller or the robot model, the robot still cannot achieve the desired attitude within acceptable error, especially the body height and roll angle.
My question is, how does the QP controller deal with such static error, when there is no integral term in the control loop? Besides, how is the performance of QP control in your hardware implementation? Can the robot stand level and still?

On the Constraint Problem of Friction Cone

Hello! Glad you can open source this project.
But, in the code, I found that the friction cone constraint equation you wrote is not the same as the one I pushed down.
Please check whether you wrote it correctly?
Below is your source code：

linear_constraints.resize(MPC_CONSTRAINT_DIM * PLAN_HORIZON, NUM_DOF * PLAN_HORIZON);    
    for (int i = 0; i < NUM_LEG * PLAN_HORIZON; ++i) {
        linear_constraints.insert(0 + 5 * i, 0 + 3 * i) = 1;
        linear_constraints.insert(1 + 5 * i, 0 + 3 * i) = 1;
        linear_constraints.insert(2 + 5 * i, 1 + 3 * i) = 1;
        linear_constraints.insert(3 + 5 * i, 1 + 3 * i) = 1;
        linear_constraints.insert(4 + 5 * i, 2 + 3 * i) = 1;

        linear_constraints.insert(0 + 5 * i, 2 + 3 * i) = mu;
        linear_constraints.insert(1 + 5 * i, 2 + 3 * i) = -mu;
        linear_constraints.insert(2 + 5 * i, 2 + 3 * i) = mu;
        linear_constraints.insert(3 + 5 * i, 2 + 3 * i) = -mu;
    }

    Eigen::VectorXd lb_one_horizon(MPC_CONSTRAINT_DIM);
    Eigen::VectorXd ub_one_horizon(MPC_CONSTRAINT_DIM);
    for (int i = 0; i < NUM_LEG; ++i) {
        lb_one_horizon.segment<5>(i * 5) << 0,
                -OsqpEigen::INFTY,
                0,
                -OsqpEigen::INFTY,
                fz_min * state.contacts[i];
        ub_one_horizon.segment<5>(i * 5) << OsqpEigen::INFTY,
                0,
                OsqpEigen::INFTY,
                0,
                fz_max * state.contacts[i];
    }
    // std:: cout << lb_one_horizon.transpose() << std::endl;
    // std:: cout << ub_one_horizon.transpose() << std::endl;
    for (int i = 0; i < PLAN_HORIZON; ++i) {
        lb.segment<MPC_CONSTRAINT_DIM>(i * MPC_CONSTRAINT_DIM) = lb_one_horizon;
        ub.segment<MPC_CONSTRAINT_DIM>(i * MPC_CONSTRAINT_DIM) = ub_one_horizon;
    }

The friction cone constraints I derived are as follows：

Looking forward to your reply！
如果可以回复中文就再好不过了

Consideration of using LCM?

Hi there,

first of all, thanks for open-sourcing the codes. It really helps the community.

I have a question about the conversion between ROS message and UDP: as described in A1 Software Developer Guide, sending UDP instructions in ROS nodes may cause communication abnormalities. To ensure real-time performance, it is recommended to use LCM server to transfer the data between ROS and UDP. But after I checked the whole code, I found that you are converting the ROS message directly to UDP and vice versa. I concern that the real-time performance cannot be ensured in this case. So I was wondering if you skip the LCM server intentionally, and if yes, why?

Many thanks.
Shengzhi Wang

roslaunch报错:ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex ERROR in osqp_setup: KKT matrix factorization. The problem seems to be non-convex. [OsqpEigen::Solver::initSolver] Unable to setup the workspace. [OsqpEigen::Solver::solveProblem] The solve has not been initialized yet. Please call initSolver() method. [OsqpEigen::Solver::solve] Unable to solve the problem.

硕哥你好，我是在自己环境下编译你的文件的，没有error．但是在roslaunch unitree_gazebo，rosrun unitree_controller unitree_servo后roslaunch a1_cpp a1_ctrl.launch就会立刻报错，信息如下：
ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex
ERROR in osqp_setup: KKT matrix factorization.
The problem seems to be non-convex.
[OsqpEigen::Solver::initSolver] Unable to setup the workspace.
[OsqpEigen::Solver::solveProblem] The solve has not been initialized yet. Please call initSolver() method.
[OsqpEigen::Solver::solve] Unable to solve the problem.

请问硕哥这是为什么

euler error and PD law for QP formulation

I was wondering why you use the rotational error phi_error = phi_des - phi? phi and phi_des are given in euler XYZ angles and therefore represent a parameterization of the rotation between the two frames (root and world). It's new to me that such an approach works. Why don't you use phi_error = R_des*R^T ? Where R_des and R, are the rotational matrices corresponding to the euler angles?

Subsequently in the PD formulation for the QP you formulated the PD law of the angular acceleration as the sum of the proportional part of the euler error (Kp * phi_error ) and the derivative part of the angular rate error in the root frame (Kd * (ang_vel_des - ang_vel)). It doesn't add up to me because you mix the euler error, which is a parameterization, with the angular rate error in the root frame.

Another quick follow up question is why you're not considering the inertia for the Quadratic program. Namely: (A*f - I * omega_dot).

Thanks guys for distributing the codebase.
cheers

a1关节位置获取延迟

你好，我遇到了一个奇怪的问题

joint_pos代表12个电机的位置，其中有几个电机，在a1_ctrl.launch启动后就有位置数据，而排位靠后的好几个电机，需要隔一段时间才突然接收到位置数据。这导致a1在初始时就站不稳。
我查了rostopic，所有电机state的数据都是正常的，应该是代码读取出现了问题。
我也尝试增加了ros::AsyncSpinner spinner的线程数，没有起到作用。
系统是ubuntu20.04。