Loading new robot (e.g. unitree go1) and starting controller and planner

AndrewZheng-1011 commented 2 years ago

Hey,

I wish to load a new robot into this framework, so to get started, I loaded unitree go1 into this framework due to the similarity with a1.

I followed the guidelines in renaming the links and joints to be of the correct format and ensuring the fixed joints on the toe is preserved when converting to sdf.
I have also edited the launch file load_robot_params.launch to include option for robot_type go1.
Additionally following #300, I set the initial position to a sitting one.

Problem

The problem I am currently having is that when I launch the command to stand, some of the links do not respond. In fact, only the hind upper legs respond. Something like this.

Attempts

I am trying to diagnose the problem and see if it is a URDF transmission tag issue (I double checked to make sure that the tag descriptions are right). I am pretty confident that the URDF and SDF is correct, although one caveat I have is that in gazebo.xacro, I had to change the robot namespace for the plugin libgazebo_ros_control.so into something like this rather than a standard / for the namespace:

<gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/robot_1</robotNamespace>
            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
        </plugin>
. . .
</gazebo>

If not, there is no response to publishing a command for it to stand or be in READY mode. Here is a comparison of some namespace issue I am running into with and without this fix when running quad_gazebo.launch

With /go1 namespace


[INFO] [1660232468.756668, 0.294000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1660232468.871810502, 0.294000000]: Loading gazebo_ros_control plugin
[ INFO] [1660232468.872104836, 0.294000000]: Starting gazebo_ros_control plugin in namespace: /go1
[ INFO] [1660232468.872734191, 0.294000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [/robot_description] on the ROS param server.
. . .
[ INFO] [1660232469.001514012, 0.294000000]: Loaded gazebo_ros_control.
[ INFO] [1660232469.491466938, 0.294000000]: Ground Truth State Estimator: <updateRateHZ> set to: 500
[ WARN] [1660232469.893714005, 0.603000000]: Leg 0 motor 0: total effort = 33.700 Nm exceeds threshold of 33.500 Nm
[ WARN] [1660232469.893786922, 0.603000000]: Leg 1 motor 0: total effort = 33.617 Nm exceeds threshold of 33.500 Nm
. . .
[WARN] [1660232497.371683, 16.591000]: Controller Spawner couldn't find the expected controller_manager ROS interface.
[robot_1/controller_spawner-8] process has finished cleanly
log file: /home/andrewzheng1011/.ros/log/02c73896-198c-11ed-8ffa-0800274207d3/robot_1-controller_spawner-8*.log

- Without /robot_1 namespace

[ INFO] [1660231534.671149971, 0.091000000]: Loading gazebo_ros_control plugin [ INFO] [1660231534.671453022, 0.091000000]: Starting gazebo_ros_control plugin in namespace: /robot_1 [ INFO] [1660231534.672041741, 0.091000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [/robot_1/robot_description] on the ROS param server. [ INFO] [1660231534.798715538, 0.091000000]: Loaded gazebo_ros_control.

So, I am not sure if this will affect anything within the code itself, but I thought this detail was worth bringing up before I address some of the other attempts in diagnosing/fixing the problem. 

For my second attempt, I see that within the code there are variables **robot_ns_** and **default_system** in the nmpc_controller cpp code (as well as similiar identification variables of what the robot in other parts of the code). I don't believe these codes will have any affect in at least trying to get the robot to stand up using the inverse dynamics controller, but just in case I added an elseif statement in the [local planner](https://github.com/robomechanics/quad-sdk/blob/b04d4f7658215da018b4843c98f49e50b1b339c6/local_planner/src/local_planner.cpp):

. . . else if (robotname == "go1") {type = A1;} // for the sake of easier integration . . .

This sadly did not work.

So, what I wish to do now is to at least try to command each joint and at least identify if commands work for each joint (Doubting if my URDF and transmission tags are good, although visually check is good, so checking through joint commands). I am launching in this manner with joint commands:

roslaunch quad_utils quad_gazebo.launch robot_type:=go1 controller:=joint # Have tried with a1 as well rostopic pub /robot_1/control/single_joint_command geometry_msgs/Vector3 "x: 0.0 y: 8.0 z: 20.0"


However, the robot does not move for leg index 0 (front left links), joint 8, for torque command 20. I have tried da few other joints, but no luck.

### Questions
So, the question I have is how can I individually control joints in quad-sdk framework to diagnose my problem? Or is there anything in the procedure that I have done that that may cause this problem?

Sorry for the long post. Wasn't sure how to concisely word this without explaining some of the roundabout methods I use.

jrenaf commented 2 years ago

Hi @AndrewZheng-1011, thanks for the detailed explanation of the issue. Would you mind attaching your sdf file of go1 here in the thread for the debugging purpose?

AndrewZheng-1011 commented 2 years ago

Sure. Seems like github does not support sdf attachments so I converted the file into a txt file. Let me know if this does not work for you.

go1.txt

jrenaf commented 2 years ago

The namespace issue is tricky. Actually we intentionally remove <robotNamespace>/robot_1</robotNamespace> from

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/robot_1</robotNamespace>
            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>

to

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>

in order to adapt to current launch file namespace framework. Have you tried this in sdf?

AndrewZheng-1011 commented 2 years ago

So, I have changed this in my sdf file (urdf and xacro as well). This alleviates the issue of labeling the namespace as either /robot_1, /go1, or etc. Though, this does not alleviate the issue of trying to get the quadruped to stand. The following result is the same as my previous post where the two hind upper leg move but nothing else.

From my perspective, it is either a urdf/sdf file issue of the transmission tags or some part of the hard code for the control to stand up. I do not see anything visually wrong with the urdf file (attached below) so I wish to use the joint controller to see if I can command each joint manually. I have tried the following commands again but it sadly did not work.

roslaunch quad_utils quad_gazebo.launch robot_type:=go1 controller:=joint # Have tried with a1 as well
rostopic pub /robot_1/control/single_joint_command geometry_msgs/Vector3 "x: 0.0
y: 8.0
z: 20.0"

Attached URDF file: go1.txt

AndrewZheng-1011 commented 2 years ago

Hey,

Some small updates from my side, but still not able to get go1 to stand up despite publishing to /robot_1/control/mode.

Joint Controller Code Changes

Problem: Current problem with joint controller problem is that the code logic linked in the main/devel code here, line 30, and highlighted down below:

. . .
if ((i == leg_idx_) && (j == joint_idx_)) {
        joint_torque_val = std::max(std::min(joint_torque_, 5.0), -5.0);
        ROS_INFO_THROTTLE(0.2, "Leg %d, joint %d, cmd = %5.3f", i, j,
                          joint_torque_val);
} else {
        joint_torque_val = 0;
}
. . .

The if conditional statement needs to be of the following:

if ((i == leg_idx_) && (joint_idx == joint_idx_)
. . .

as values of j $\in$ [0, 2] when jointidx $\in$ [0,11], which makes it impossible to get any joints commands other than legidx = 0 (front left).

Joint Controller Tests w/ Spirit and Go1

I have conducted some joint control test with the following updated joint controller code. Each of these joints move in an expected manner (lower joints with opposite rotation due to axis of rotation set to negative... A1 and Go1 both have this orientation but I have not verified for spirit). The test I have conducted is shown in the txt file: joint_controller_notes.txt

So, from this test, I am pretty positive it is not a URDF/SDF issue. However, I am still working on figuring the root of the issue when trying to get the robot to stand up. I will also be trying to try this work in the devel/ branch as I was mainly working on the main branch.

Additionally, for the future, is there a way to ask for PR or is this still an issue as conversated in PR #290?

jrenaf commented 2 years ago

Hmm, it's wierd that only hinder legs react but not front legs. Are all joints actuated in hinder legs? You can also try to print values in controller plugin function update to check whether it's been assigned correctly.

AndrewZheng-1011 commented 2 years ago

So, I managed to figure out the issue after a few series of test runs. The root of the problem was that the robot spawned with the upper leg and lower legs within one another in the sitting position. I am assuming some weird simulation interaction causes the robot to not be able to stand up, but after changing the initial origin of the rpy of knee joint angle from (0, -PI, 0) to (0, -PI/1.03, 0), the simulation for loading, standing, and executing planners (global, local, and leg controllers) ran successfully.

Before I close this issue page, could I get some insight if loading an even smaller robot (half the size of a1) will work with this framework. Mainly, my question concerns with specific configuration such as the NMPC controller. I see that there are generated C codes for A1 and spirit. I am under the assumption that I need specific C code generation for different robot. The success of my simulation I attribute to its close resemblance of A1. I just want to verify if you had any insights on this before I invest some time into doing this work.

ardalantj commented 2 years ago

I am glad your problem is resolved.

You can certainly add a smaller robot similar to spirit and a1 provided the generated dynamics equations are updated. Checkout the scripts folder in nmpc package for details on what parameters are necessary and let us know if you ran into any issues. If you have got another robot working, feel free to open a PR !

AndrewZheng-1011 commented 2 years ago

Thanks for letting me know. I will work on that later this week. Currently, I will still need to update the generated dynamic equations for go1 for the nmpc before asking for a PR. Hopefully won't take too long. I will pose any troubles I have along the way, and perhaps I can update the wiki page for loading a new robot such that the experience is a little bit smoother.

ardalantj commented 2 years ago

Sounds great. Will close this issue. Feel free to open a new one if you had any other issues.

robomechanics / quad-sdk