Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #2276 from erwincoumans/master

Browse Source 
add toes to Mini Cheetah
This commit is contained in:
erwincoumans
2019-06-02 18:29:29 -04:00
committed by GitHub
parent 4d3ee40895 d7c0c9c017
commit 500264ca23
8 changed files with 292 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
examples/InverseKinematics/InverseKinematicsExample.cpp
View File

@@ -72,18 +72,22 @@ void Reset(Tree& tree, Jacobian* m_ikJacobian)
// Update target positions
void UpdateTargets(double T2, Tree& treeY)
void UpdateTargets(double T, Tree& treeY)
{
double T = T2 / 5.;
targetaa[0].Set(0.6 * b3Sin(0), 0.6 * b3Cos(0), 0.5 + 0.4 * b3Sin(3 * T));
targetaa[0].Set(2.0f + 1.5*sin(3 * T) * 2, -0.5 + 1.0f + 0.2*sin(7 * T) * 2, 0.3f + 0.7*sin(5 * T) * 2);
targetaa[1].Set(0.5f + 0.4*sin(4 * T) * 2, -0.5 + 0.9f + 0.3*sin(4 * T) * 2, -0.2f + 1.0*sin(3 * T) * 2);
targetaa[2].Set(-0.5f + 0.8*sin(6 * T) * 2, -0.5 + 1.1f + 0.2*sin(7 * T) * 2, 0.3f + 0.5*sin(8 * T) * 2);
targetaa[3].Set(-1.6f + 0.8*sin(4 * T) * 2, -0.5 + 0.8f + 0.3*sin(4 * T) * 2, -0.2f + 0.3*sin(3 * T) * 2);
}
// Does a single update (on one kind of tree)
// Does a single update (on one kind of m_ikTree)
void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
{
B3_PROFILE("IK_DoUpdateStep");
if (SleepCounter == 0)
{
T += Tstep;
T += Tstep*0.1;
UpdateTargets(T, treeY);
}
@@ -142,7 +146,7 @@ class InverseKinematicsExample : public CommonExampleInterface
Jacobian* m_ikJacobian;
b3AlignedObjectArray<int> m_movingInstances;
int m_targetInstance;
b3AlignedObjectArray<int> m_targetInstances;
enum
{
numCubesX = 20,
@@ -152,10 +156,9 @@ class InverseKinematicsExample : public CommonExampleInterface
public:
InverseKinematicsExample(CommonGraphicsApp* app, int option)
: m_app(app),
m_ikMethod(option),
m_targetInstance(-1)
m_ikMethod(option)
{
m_app->setUpAxis(2);
m_app->setUpAxis(1);
{
b3Vector3 extents = b3MakeVector3(100, 100, 100);
@@ -166,7 +169,7 @@ public:
b3Vector4 color0 = b3MakeVector4(0.4, 0.4, 0.4, 1);
b3Vector4 color1 = b3MakeVector4(0.6, 0.6, 0.6, 1);
m_app->registerGrid(xres, yres, color0, color1);
//m_app->registerGrid(xres, yres, color0, color1);
}
///create some graphics proxy for the tracking target
@@ -177,8 +180,11 @@ public:
pos[app->getUpAxis()] = 1;
b3Quaternion orn(0, 0, 0, 1);
b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
m_targetInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling);
b3Vector3 scaling = b3MakeVector3(.1, .1, .1);
m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
}
m_app->m_renderer->writeTransforms();
}
@@ -215,17 +221,17 @@ public:
act.setRotation(rot);
act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z));
}
void MyDrawTree(Node* node, const b3Transform& tr)
void MyDrawTree(Node* node, const b3Transform& tr, const b3Transform& parentTr)
{
b3Vector3 lineColor = b3MakeVector3(0, 0, 0);
int lineWidth = 2;
if (node != 0)
{
// glPushMatrix();
b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z);
b3Vector3 color = b3MakeVector3(0, 1, 0);
b3Vector3 color1 = b3MakeVector3(0, 1, 0);
int pointSize = 10;
m_app->m_renderer->drawPoint(pos, color, pointSize);
m_app->m_renderer->drawPoint(pos, color1, pointSize);
m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(0), b3MakeVector3(1, 0, 0), lineWidth);
m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth);
@@ -236,25 +242,29 @@ public:
m_app->m_renderer->drawLine(pos, pos + 0.1 * axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5);
//node->DrawNode(node == root); // Recursively draw node and update ModelView matrix
if (node->left)
{
b3Transform act;
getLocalTransform(node->left, act);
b3Transform trl = tr * act;
m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth);
MyDrawTree(node->left, trl); // Draw tree of children recursively
}
// glPopMatrix();
if (node->right)
{
b3Transform act;
getLocalTransform(node->right, act);
b3Transform trr = tr * act;
m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth);
MyDrawTree(node->right, trr); // Draw right siblings recursively
b3Transform ptrr = parentTr * act;
b3Vector3 lineColor = b3MakeVector3(0, 1, 0);
m_app->m_renderer->drawLine(tr.getOrigin(), ptrr.getOrigin(), lineColor, lineWidth);
MyDrawTree(node->right, ptrr, parentTr); // Draw right siblings recursively
}
//node->DrawNode(node == root); // Recursively draw node and update ModelView matrix
if (node->left)
{
b3Transform act;
getLocalTransform(node->left, act);
b3Vector3 lineColor = b3MakeVector3(1, 0, 0);
b3Transform trl = tr * act;
m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth);
MyDrawTree(node->left, trl, tr); // Draw m_ikTree of children recursively
}
}
}
virtual void stepSimulation(float deltaTime)
@@ -265,12 +275,15 @@ public:
{
b3Transform act;
getLocalTransform(m_ikTree.GetRoot(), act);
MyDrawTree(m_ikTree.GetRoot(), act);
MyDrawTree(m_ikTree.GetRoot(), act, b3Transform::getIdentity());
b3Vector3 pos = b3MakeVector3(targetaa[0].x, targetaa[0].y, targetaa[0].z);
b3Quaternion orn(0, 0, 0, 1);
for (int i = 0; i < m_targetInstances.size(); i++)
{
b3Vector3 pos = b3MakeVector3(targetaa[i].x, targetaa[i].y, targetaa[i].z);
b3Quaternion orn(0, 0, 0, 1);
m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance);
m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstances[i]);
}
m_app->m_renderer->writeTransforms();
m_app->m_renderer->renderScene();
}
@@ -309,40 +322,117 @@ public:
void InverseKinematicsExample::BuildKukaIIWAShape()
{
//const VectorR3& unitx = VectorR3::UnitX;
m_ikNodes.resize(29);
const VectorR3& unitx = VectorR3::UnitX;
const VectorR3& unity = VectorR3::UnitY;
const VectorR3& unitz = VectorR3::UnitZ;
const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
const VectorR3& zero = VectorR3::Zero;
VectorR3 p0(0.0f, -1.5f, 0.0f);
VectorR3 p1(0.0f, -1.0f, 0.0f);
VectorR3 p2(0.0f, -0.5f, 0.0f);
VectorR3 p3(0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f);
VectorR3 p4(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f);
VectorR3 p5(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f);
VectorR3 p6(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f);
VectorR3 p7(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f);
VectorR3 p8(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f);
VectorR3 p9(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f);
VectorR3 p10(-0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f);
VectorR3 p11(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f);
VectorR3 p12(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f);
VectorR3 p13(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f);
VectorR3 p14(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f);
VectorR3 p15(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f);
VectorR3 p16(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f);
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_ikNodes.resize(8); //7DOF+additional endeffector
m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
m_ikNodes[0] = new Node(p0, unit1, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertRoot(m_ikNodes[0]);
m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
m_ikNodes[1] = new Node(p1, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]);
m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikNodes[2] = new Node(p1, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]);
m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikNodes[3] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]);
m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[4]);
m_ikNodes[4] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertRightSibling(m_ikNodes[3], m_ikNodes[4]);
m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[5]);
m_ikNodes[5] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[5]);
m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[6]);
m_ikNodes[6] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertRightSibling(m_ikNodes[5], m_ikNodes[6]);
m_ikNodes[7] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[7]);
m_ikNodes[8] = new Node(p4, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[7], m_ikNodes[8]);
m_ikNodes[9] = new Node(p5, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[8], m_ikNodes[9]);
m_ikNodes[10] = new Node(p5, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[9], m_ikNodes[10]);
m_ikNodes[11] = new Node(p6, zero, 0.08, EFFECTOR);
m_ikTree.InsertLeftChild(m_ikNodes[10], m_ikNodes[11]);
m_ikNodes[12] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[12]);
m_ikNodes[13] = new Node(p7, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[12], m_ikNodes[13]);
m_ikNodes[14] = new Node(p8, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[13], m_ikNodes[14]);
m_ikNodes[15] = new Node(p8, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[14], m_ikNodes[15]);
m_ikNodes[16] = new Node(p9, zero, 0.08, EFFECTOR);
m_ikTree.InsertLeftChild(m_ikNodes[15], m_ikNodes[16]);
m_ikNodes[17] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[17]);
m_ikNodes[18] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[17], m_ikNodes[18]);
m_ikNodes[19] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertRightSibling(m_ikNodes[17], m_ikNodes[19]);
m_ikNodes[20] = new Node(p11, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[18], m_ikNodes[20]);
m_ikNodes[21] = new Node(p12, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[20], m_ikNodes[21]);
m_ikNodes[22] = new Node(p12, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[21], m_ikNodes[22]);
m_ikNodes[23] = new Node(p13, zero, 0.08, EFFECTOR);
m_ikTree.InsertLeftChild(m_ikNodes[22], m_ikNodes[23]);
m_ikNodes[24] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[19], m_ikNodes[24]);
m_ikNodes[25] = new Node(p14, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[24], m_ikNodes[25]);
m_ikNodes[26] = new Node(p15, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[25], m_ikNodes[26]);
m_ikNodes[27] = new Node(p15, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
m_ikTree.InsertLeftChild(m_ikNodes[26], m_ikNodes[27]);
m_ikNodes[28] = new Node(p16, zero, 0.08, EFFECTOR);
m_ikTree.InsertLeftChild(m_ikNodes[27], m_ikNodes[28]);
m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[7]);
}
class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options)

4
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -472,8 +472,8 @@ void Jacobian::CalcDeltaThetasSDLS()
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
int nRows = J.GetNumRows();
// TODO: Modify it to work with multiple end effectors.
int numEndEffectors = 1;
int numEndEffectors = m_tree->GetNumEffector();
int nCols = J.GetNumColumns();
dTheta.SetZero();

133
examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf
View File

@@ -100,7 +100,40 @@
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_fr">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_fr_joint" type="fixed">
<parent link="shank_fr"/>
<child link="toe_fr"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Front Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_fl_j" type="continuous">
<axis xyz="1 0 0"/>
@@ -181,6 +214,39 @@
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_fl">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_fl_joint" type="fixed">
<parent link="shank_fl"/>
<child link="toe_fl"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Hind Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_hr_j" type="continuous">
@@ -262,6 +328,39 @@
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_hr">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_hr_joint" type="fixed">
<parent link="shank_hr"/>
<child link="toe_hr"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Hind Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_hl_j" type="continuous">
@@ -343,5 +442,39 @@
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_hl">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_hl_joint" type="fixed">
<parent link="shank_hl"/>
<child link="toe_hl"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
</robot>

6
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -311,9 +311,9 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
case RO_XYZ:
{
//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
//The two planes are non-homologous, so this is a Tait<EFBFBD>Bryan angle formalism and not a proper Euler
//The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler
//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait<EFBFBD>Bryan angles)
//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles)
// x' = Nperp = N.cross(axis2)
// y' = N = axis2.cross(axis0)
// z' = z
@@ -866,7 +866,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
// vel + f / m * (rotational ? -1 : 1)
// so in theory this should be set here for m_constraintError
// (with m_constraintError we set a desired velocity for the affected body(es))
// however in practice any value is fine as long as it is greater then the "proper" velocity,
// however in practice any value is fine as long as it is greater than the "proper" velocity,
// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
// (Even with our best intent the "new" velocity is only an estimation. If we underestimate

2
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
View File

@@ -294,7 +294,7 @@ protected:
bool m_hasStaticBody;
int m_flags;
btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&)
btGeneric6DofSpring2Constraint& operator=(const btGeneric6DofSpring2Constraint&)
{
btAssert(0);
return *this;

4
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -352,9 +352,9 @@ void btMultiBody::finalizeMultiDof()
updateLinksDofOffsets();
}
int btMultiBody::getParent(int i) const
int btMultiBody::getParent(int link_num) const
{
return m_links[i].m_parent;
return m_links[link_num].m_parent;
}
btScalar btMultiBody::getLinkMass(int i) const

20
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -65,7 +65,7 @@ public:
virtual ~btMultiBody();
//note: fixed link collision with parent is always disabled
void setupFixed(int linkIndex,
void setupFixed(int i, //linkIndex
btScalar mass,
const btVector3 &inertia,
int parent,
@@ -83,7 +83,7 @@ public:
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);
void setupRevolute(int linkIndex, // 0 to num_links-1
void setupRevolute(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parentIndex,
@@ -93,7 +93,7 @@ public:
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision = false);
void setupSpherical(int linkIndex, // 0 to num_links-1
void setupSpherical(int i, // linkIndex, 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
@@ -277,15 +277,15 @@ public:
//
// transform vectors in local frame of link i to world frame (or vice versa)
//
btVector3 localPosToWorld(int i, const btVector3 &vec) const;
btVector3 localDirToWorld(int i, const btVector3 &vec) const;
btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const;
btVector3 localDirToWorld(int i, const btVector3 &local_dir) const;
btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const;
btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const;
//
// transform a frame in local coordinate to a frame in world coordinate
//
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
//
// calculate kinetic energy and angular momentum
@@ -576,11 +576,11 @@ public:
{
return m_internalNeedsJointFeedback;
}
void forwardKinematics(btAlignedObjectArray<btQuaternion> & scratch_q, btAlignedObjectArray<btVector3> & scratch_m);
void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin);
void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & scratch_q, btAlignedObjectArray<btVector3> & scratch_m);
void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
virtual int calculateSerializeBufferSize() const;

2
src/LinearMath/btMatrixX.h
View File

@@ -284,7 +284,7 @@ struct btMatrixX
}
}
void printMatrix(const char* msg)
void printMatrix(const char* msg) const
{
printf("%s ---------------------\n", msg);
for (int i = 0; i < rows(); i++)