Hello, thanks for the work.
I want to simulate a humanoid using raisim, but encounter a strange problem. After loading the urdf, the 'setState' is ok, kinematic part seems fine, playing mocap data looks no problem. But when call 'world.integrate()' whether with pd or not, the joint configurations are always 'nan' (testing on lots of initial states), if just call 'world.integrate1()' and run 'getInverseMassMatrix' returns 'Nan' array alreadly while the 'MassMatrix' looks reasonable and is invertible.
After doing some trials, found something related to the urdf, joint/link definition seems to be the reason, change 'xyz' on some joint origin or mass/inertia on link to bigger values (which is apparently wrong value) may not encounter the problem. Don't know why, its this related to the 'unrealistic internal collisions' ? Or how do i make sure a right difinition?
The urdf comes from the bullet example with link inertia computed, and no big value diff except lots of 'spherical' joint comparing with the raisim anymal example urdf, same file on pybullet, things are ok.
playing mocap:
a small test script:
#include "raisim/World.hpp"
int main(){
raisim::World world;
auto robot = world.addArticulatedSystem("../../data/characters/humanoid.urdf");
auto ground = world.addGround();
std::cout<<robot->getGeneralizedCoordinate()<<std::endl;
world.setTimeStep(0.002);
world.integrate();
std::cout<<robot->getGeneralizedCoordinate()<<std::endl;
}
Below is part of the humanoid.urdf file, borrowed from the 'bullet' repository example, when loading this, world.integrate produce 'nan's but change 'right_hip' joints' origin to origin rpy="0 0 0" xyz="0 1 0.085" no 'nan' s.
<?xml version="1.0"?>
<robot name="dumpUrdf">
<link name="base">
</link>
<!-- Root -->
<link name="root">
<inertial>
<origin rpy = "0 0 0" xyz = "0.000000 0.070000 0.000000" />
<mass value = "12.000000" />
<inertia ixx = "0.03888" ixy = "0" ixz = "0" iyy = "0.03888" iyz = "0" izz = "0.03888" />
</inertial>
<collision>
<origin rpy = "0 0 0" xyz = "0.000000 0.070000 0.000000" />
<geometry>
<sphere radius = "0.090000" />
</geometry>
</collision>
<visual>
<origin rpy = "0 0 0" xyz = "0.000000 0.070000 0.000000" />
<geometry>
<sphere radius = "0.090000" />
</geometry>
</visual>
</link>
<joint name="root" type="fixed">
<parent link="base" />
<child link="root" />
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.0000" />
</joint>
<!-- Chest -->
<link name="chest">
<inertial>
<origin rpy = "0 0 0" xyz = "0.000000 0.120000 0.000000" />
<mass value = "28.000000" />
<inertia ixx = "0.1552" ixy = "0" ixz = "0" iyy = "0.1552" iyz = "0" izz = "0.1552" />
</inertial>
<collision>
<origin rpy = "0 0 0" xyz = "0.000000 0.120000 0.000000" />
<geometry>
<sphere radius = "0.110000" />
</geometry>
</collision>
<visual>
<origin rpy = "0 0 0" xyz = "0.000000 0.120000 0.000000" />
<geometry>
<sphere radius = "0.110000" />
</geometry>
</visual>
</link>
<joint name="chest" type="spherical">
<parent link="root" />
<child link="chest" />
<origin rpy = "0 0 0" xyz = "0.000000 0.234 0.000000" />
</joint>
<!-- Neck -->
<link name="neck">
<inertial>
<origin rpy = "0 0 0" xyz = "0.000000 0.17500000 0.000000" />
<mass value = "4.000000" />
<inertia ixx = "0.0174" ixy = "0" ixz = "0" iyy = "0.0174" iyz = "0" izz = "0.0174" />
</inertial>
<collision>
<origin rpy = "0 0 0" xyz = "0.000000 0.1750000 0.000000" />
<geometry>
<sphere radius = "0.1050000" />
</geometry>
</collision>
<visual>
<origin rpy = "0 0 0" xyz = "0.000000 0.17500000 0.000000" />
<geometry>
<sphere radius = "0.1050000" />
</geometry>
</visual>
</link>
<joint name="neck" type="spherical">
<parent link="chest" />
<child link="neck" />
<origin rpy = "0 0 0" xyz = "0.000000 0.224 0.000000" />
</joint>
<!-- Right Hip -->
<joint name="right_hip" type="spherical">
<parent link="root" />
<child link="right_hip" />
<origin rpy="0 0 0" xyz="0 0 0.085" />
</joint>
<link name="right_hip">
<inertial>
<origin rpy = "0 0 0" xyz = "0.000000 -0.240000 0" />
<mass value = "9.00000" />
<inertia ixx = "0.126" ixy = "0" ixz = "0" iyy = "0.0138" iyz = "0" izz = "0.126" />
</inertial>
<collision>
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.240000 0.000000" />
<geometry>
<capsule length="0.1200000" radius="0.055000"/>
</geometry>
</collision>
<visual>
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.240000 0.000000" />
<geometry>
<capsule length="0.1200000" radius="0.0550000"/>
</geometry>
</visual>
</link>
</robot>