Hey there!
Sorry for the late reply; I was been busy with this semesters start.
Today I found time to tinker around with it again and settled for hinge constraints. My goal was to get just one hinge to work.
Let's suppose we have the following URDF definition (just an excerpt from bullet's kuka.urdf):
Code: Select all
<link name="calib_kuka_arm_base_link">
<inertial>
<mass value="0"/>
<!-- static base, disable dynamics for this link -->
<origin rpy="0 0 0" xyz="0 0 0.055"/>
<inertia ixx="0.00381666666667" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.00381666666667"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="assets/models/kuka_lwr/arm_base.ply"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="assets/models/kuka_lwr/arm_base_convex.ply"/>
</geometry>
</collision>
</link>
<joint name="kuka_arm_0_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.11"/>
<axis xyz="0 0 1"/>
<limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="1.91986217719"/>
<dynamics damping="0.1"/>
<parent link="calib_kuka_arm_base_link"/>
<child link="kuka_arm_1_link"/>
</joint>
<link name="kuka_arm_1_link">
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0.04 0.130"/>
<inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.0118666666667" iyz="0" izz="0.003"/>
</inertial>
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="assets/models/kuka_lwr/arm_segment_a.ply"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="assets/models/kuka_lwr/arm_segment_a_convex.ply"/>
</geometry>
</collision>
</link>
The way I understood the URDF specification, the pivot in a would be (0, 0, 0.11), pivot in b would be(0, 0, 0) and the axis in a and b (0, 0, 1).
So I assumed, that following code would work:
Code: Select all
for (std::size_t it = 1; it < link_count; ++it) {
const auto& link = _robot->link(it);
const auto& joint = link.parent_joint();
btVector3 pivot_in_b = joint.origin().getOrigin(); // Specifies the offset off the joint in its local frame; here (0, 0, 0.11)
btVector3 pivot_in_a = { 0, 0, 0 }; // Is this assumption right?
// TODO: Offset collision shape???
auto hinge = new btHingeConstraint(
*_rigid_bodies[it - 1],
*_rigid_bodies[it],
pivot_in_a,
pivot_in_b,
joint.axis(),
joint.axis()
);
hinge->enableMotor(true);
hinge->setMaxMotorImpulse(1000);
auto component = new constraint_component{};
component->constraint = hinge;
_entities[it].add_component(component);
}
Is this the right assumption? If not, how would I calculate the pivot points and axis?