Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
bla
@@ -1,6 +1,6 @@
|
||||
SUBDIRS( HelloWorld BasicDemo )
|
||||
IF(BUILD_BULLET3)
|
||||
SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint )
|
||||
SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint c_api)
|
||||
ENDIF()
|
||||
|
||||
IF(BUILD_PYBULLET)
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_INCLINED_PLANE_EXAMPLE_H
|
||||
#define ET_INCLINED_PLANE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_INCLINED_PLANE_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_INCLINED_PLANE_EXAMPLE_H
|
||||
#define ET_INCLINED_PLANE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_INCLINED_PLANE_EXAMPLE_H
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
#define ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
#define ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
|
||||
@@ -9,9 +9,9 @@
|
||||
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||
#include "../ImportURDFDemo/UrdfFindMeshFile.h"
|
||||
#include <string>
|
||||
#include "../../Utils/b3ResourcePath.h"
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include "../../Utils/b3ResourcePath.h"
|
||||
#include "../ImportURDFDemo/URDF2Bullet.h"
|
||||
#include "../ImportURDFDemo/UrdfParser.h"
|
||||
#include "../ImportURDFDemo/urdfStringSplit.h"
|
||||
@@ -1453,17 +1453,15 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
|
||||
}
|
||||
else
|
||||
{
|
||||
int maxPathLen = 1024;
|
||||
fu.extractPath(relativeFileName, m_data->m_pathPrefix, maxPathLen);
|
||||
//read file
|
||||
int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
|
||||
|
||||
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||
while (xml_file.good())
|
||||
char destBuffer[8192];
|
||||
while (m_data->m_fileIO->readLine(fileId, destBuffer, 8192))
|
||||
{
|
||||
std::string line;
|
||||
std::getline(xml_file, line);
|
||||
xml_string += (line + "\n");
|
||||
xml_string += (std::string(destBuffer) + "\n");
|
||||
}
|
||||
xml_file.close();
|
||||
m_data->m_fileIO->fileClose(fileId);
|
||||
|
||||
if (parseMJCFString(xml_string.c_str(), logger))
|
||||
{
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
210
examples/c_api/CMakeLists.txt
Normal file
210
examples/c_api/CMakeLists.txt
Normal file
@@ -0,0 +1,210 @@
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
|
||||
)
|
||||
|
||||
|
||||
SET(
|
||||
bullet_c_api_SRCS
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h
|
||||
../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
|
||||
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
|
||||
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
|
||||
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
|
||||
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
|
||||
../../examples/SharedMemory/IKTrajectoryHelper.cpp
|
||||
../../examples/SharedMemory/IKTrajectoryHelper.h
|
||||
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.h
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h
|
||||
../../examples/OpenGLWindow/SimpleCamera.cpp
|
||||
../../examples/OpenGLWindow/SimpleCamera.h
|
||||
../../examples/TinyRenderer/geometry.cpp
|
||||
../../examples/TinyRenderer/model.cpp
|
||||
../../examples/TinyRenderer/tgaimage.cpp
|
||||
../../examples/TinyRenderer/our_gl.cpp
|
||||
../../examples/TinyRenderer/TinyRenderer.cpp
|
||||
../../examples/SharedMemory/InProcessMemory.cpp
|
||||
../../examples/SharedMemory/PhysicsClient.cpp
|
||||
../../examples/SharedMemory/PhysicsClient.h
|
||||
../../examples/SharedMemory/PhysicsServer.cpp
|
||||
../../examples/SharedMemory/PhysicsServer.h
|
||||
../../examples/SharedMemory/PhysicsServerExample.cpp
|
||||
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
|
||||
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
|
||||
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
|
||||
../../examples/SharedMemory/PhysicsServerSharedMemory.h
|
||||
../../examples/SharedMemory/PhysicsDirect.cpp
|
||||
../../examples/SharedMemory/PhysicsDirect.h
|
||||
../../examples/SharedMemory/PhysicsDirectC_API.cpp
|
||||
../../examples/SharedMemory/PhysicsDirectC_API.h
|
||||
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
|
||||
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
|
||||
../../examples/SharedMemory/b3PluginManager.cpp
|
||||
../../examples/SharedMemory/b3PluginManager.h
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory.h
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
|
||||
../../examples/SharedMemory/PhysicsClientC_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientC_API.h
|
||||
../../examples/SharedMemory/Win32SharedMemory.cpp
|
||||
../../examples/SharedMemory/Win32SharedMemory.h
|
||||
../../examples/SharedMemory/PosixSharedMemory.cpp
|
||||
../../examples/SharedMemory/PosixSharedMemory.h
|
||||
../../examples/Utils/b3ResourcePath.cpp
|
||||
../../examples/Utils/b3ResourcePath.h
|
||||
../../examples/Utils/RobotLoggingUtil.cpp
|
||||
../../examples/Utils/RobotLoggingUtil.h
|
||||
../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
|
||||
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
|
||||
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
|
||||
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
|
||||
../../examples/ThirdPartyLibs/stb_image/stb_image_write.cpp
|
||||
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
|
||||
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
|
||||
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
|
||||
../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
|
||||
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
|
||||
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
|
||||
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
|
||||
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
|
||||
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
|
||||
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
|
||||
../../examples/MultiThreading/b3PosixThreadSupport.cpp
|
||||
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
|
||||
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
|
||||
)
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
|
||||
ENDIF(BUILD_PYBULLET_CLSOCKET)
|
||||
|
||||
IF(WIN32)
|
||||
IF(BUILD_PYBULLET_ENET)
|
||||
ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET)
|
||||
ENDIF(BUILD_PYBULLET_ENET)
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
ADD_DEFINITIONS(-DWIN32)
|
||||
ENDIF(BUILD_PYBULLET_CLSOCKET)
|
||||
|
||||
ELSE(WIN32)
|
||||
IF(BUILD_PYBULLET_ENET)
|
||||
ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET)
|
||||
ENDIF(BUILD_PYBULLET_ENET)
|
||||
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
ADD_DEFINITIONS(${OSDEF})
|
||||
ENDIF(BUILD_PYBULLET_CLSOCKET)
|
||||
ENDIF(WIN32)
|
||||
|
||||
IF(BUILD_PYBULLET_ENET)
|
||||
SET(
|
||||
bullet_c_api_SRCS
|
||||
${bullet_c_api_SRCS}
|
||||
../../examples/SharedMemory/PhysicsClientUDP.cpp
|
||||
../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientUDP.h
|
||||
../../examples/SharedMemory/PhysicsClientUDP_C_API.h
|
||||
../../examples/ThirdPartyLibs/enet/win32.c
|
||||
../../examples/ThirdPartyLibs/enet/unix.c
|
||||
../../examples/ThirdPartyLibs/enet/callbacks.c
|
||||
../../examples/ThirdPartyLibs/enet/compress.c
|
||||
../../examples/ThirdPartyLibs/enet/host.c
|
||||
../../examples/ThirdPartyLibs/enet/list.c
|
||||
../../examples/ThirdPartyLibs/enet/packet.c
|
||||
../../examples/ThirdPartyLibs/enet/peer.c
|
||||
../../examples/ThirdPartyLibs/enet/protocol.c
|
||||
)
|
||||
ENDIF(BUILD_PYBULLET_ENET)
|
||||
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
SET(
|
||||
bullet_c_api_SRCS
|
||||
${bullet_c_api_SRCS}
|
||||
../../examples/SharedMemory/PhysicsClientTCP.cpp
|
||||
../../examples/SharedMemory/PhysicsClientTCP.h
|
||||
../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientTCP_C_API.h
|
||||
../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp
|
||||
../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp
|
||||
../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp
|
||||
)
|
||||
ENDIF()
|
||||
|
||||
ADD_LIBRARY(bullet_c_api SHARED ${bullet_c_api_SRCS})
|
||||
|
||||
SET_TARGET_PROPERTIES(bullet_c_api PROPERTIES PREFIX "")
|
||||
SET_TARGET_PROPERTIES(bullet_c_api PROPERTIES POSTFIX "")
|
||||
|
||||
SET_TARGET_PROPERTIES(bullet_c_api PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(bullet_c_api PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(bullet_c_api PROPERTIES DEBUG_POSTFIX "_d")
|
||||
|
||||
IF(WIN32)
|
||||
IF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
|
||||
TARGET_LINK_LIBRARIES(bullet_c_api ws2_32)
|
||||
ENDIF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
|
||||
ENDIF(WIN32)
|
||||
|
||||
|
||||
TARGET_LINK_LIBRARIES(
|
||||
bullet_c_api
|
||||
BulletExampleBrowserLib
|
||||
BulletFileLoader
|
||||
BulletWorldImporter
|
||||
BulletSoftBody
|
||||
BulletDynamics
|
||||
BulletCollision
|
||||
BulletInverseDynamicsUtils
|
||||
BulletInverseDynamics
|
||||
LinearMath
|
||||
OpenGLWindow
|
||||
gwen
|
||||
BussIK
|
||||
Bullet3Common
|
||||
)
|
||||
|
||||
TARGET_LINK_LIBRARIES(bullet_c_api ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
|
||||
|
||||
IF (INSTALL_LIBS)
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
#INSTALL of other files requires CMake 2.6
|
||||
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS bullet_c_api DESTINATION lib)
|
||||
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS bullet_c_api RUNTIME DESTINATION bin
|
||||
LIBRARY DESTINATION lib${LIB_SUFFIX}
|
||||
ARCHIVE DESTINATION lib${LIB_SUFFIX})
|
||||
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
SET_TARGET_PROPERTIES(bullet_c_api PROPERTIES FRAMEWORK true)
|
||||
|
||||
SET_TARGET_PROPERTIES(bullet_c_api PROPERTIES PUBLIC_HEADER "${Root_HDRS}")
|
||||
# Have to list out sub-directories manually:
|
||||
|
||||
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
ENDIF (INSTALL_LIBS)
|
||||
@@ -1,165 +1,21 @@
|
||||
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
|
||||
${PYTHON_INCLUDE_DIRS}
|
||||
)
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
|
||||
${PYTHON_INCLUDE_DIRS}
|
||||
)
|
||||
IF(BUILD_PYBULLET_NUMPY)
|
||||
INCLUDE_DIRECTORIES(
|
||||
${PYTHON_NUMPY_INCLUDE_DIR}
|
||||
)
|
||||
INCLUDE_DIRECTORIES(${PYTHON_NUMPY_INCLUDE_DIR})
|
||||
ENDIF()
|
||||
|
||||
ADD_DEFINITIONS(-DSTATIC_LINK_SPD_PLUGIN)
|
||||
|
||||
SET(pybullet_SRCS
|
||||
pybullet.c
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.h
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp
|
||||
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h
|
||||
../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
|
||||
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
|
||||
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
|
||||
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
|
||||
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
|
||||
../../examples/SharedMemory/IKTrajectoryHelper.cpp
|
||||
../../examples/SharedMemory/IKTrajectoryHelper.h
|
||||
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.h
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
|
||||
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h
|
||||
../../examples/OpenGLWindow/SimpleCamera.cpp
|
||||
../../examples/OpenGLWindow/SimpleCamera.h
|
||||
../../examples/TinyRenderer/geometry.cpp
|
||||
../../examples/TinyRenderer/model.cpp
|
||||
../../examples/TinyRenderer/tgaimage.cpp
|
||||
../../examples/TinyRenderer/our_gl.cpp
|
||||
../../examples/TinyRenderer/TinyRenderer.cpp
|
||||
../../examples/SharedMemory/InProcessMemory.cpp
|
||||
../../examples/SharedMemory/PhysicsClient.cpp
|
||||
../../examples/SharedMemory/PhysicsClient.h
|
||||
../../examples/SharedMemory/PhysicsServer.cpp
|
||||
../../examples/SharedMemory/PhysicsServer.h
|
||||
../../examples/SharedMemory/PhysicsServerExample.cpp
|
||||
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
|
||||
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
|
||||
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
|
||||
../../examples/SharedMemory/PhysicsServerSharedMemory.h
|
||||
../../examples/SharedMemory/PhysicsDirect.cpp
|
||||
../../examples/SharedMemory/PhysicsDirect.h
|
||||
../../examples/SharedMemory/PhysicsDirectC_API.cpp
|
||||
../../examples/SharedMemory/PhysicsDirectC_API.h
|
||||
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
|
||||
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
|
||||
../../examples/SharedMemory/b3PluginManager.cpp
|
||||
../../examples/SharedMemory/b3PluginManager.h
|
||||
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory.h
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
|
||||
|
||||
../../examples/SharedMemory/PhysicsClientC_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientC_API.h
|
||||
../../examples/SharedMemory/Win32SharedMemory.cpp
|
||||
../../examples/SharedMemory/Win32SharedMemory.h
|
||||
../../examples/SharedMemory/PosixSharedMemory.cpp
|
||||
../../examples/SharedMemory/PosixSharedMemory.h
|
||||
../../examples/Utils/b3ResourcePath.cpp
|
||||
../../examples/Utils/b3ResourcePath.h
|
||||
../../examples/Utils/RobotLoggingUtil.cpp
|
||||
../../examples/Utils/RobotLoggingUtil.h
|
||||
|
||||
../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
|
||||
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
|
||||
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
|
||||
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
|
||||
../../examples/ThirdPartyLibs/stb_image/stb_image_write.cpp
|
||||
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
|
||||
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
|
||||
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
|
||||
../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
|
||||
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
|
||||
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
|
||||
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
|
||||
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
|
||||
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
|
||||
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
|
||||
../../examples/MultiThreading/b3PosixThreadSupport.cpp
|
||||
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
|
||||
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
|
||||
|
||||
SET(
|
||||
pybullet_SRCS
|
||||
pybullet.c
|
||||
)
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
|
||||
ENDIF(BUILD_PYBULLET_CLSOCKET)
|
||||
|
||||
IF(WIN32)
|
||||
LINK_LIBRARIES(
|
||||
${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
|
||||
)
|
||||
IF(BUILD_PYBULLET_ENET)
|
||||
ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET)
|
||||
ENDIF(BUILD_PYBULLET_ENET)
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
ADD_DEFINITIONS(-DWIN32)
|
||||
ENDIF(BUILD_PYBULLET_CLSOCKET)
|
||||
|
||||
ELSE(WIN32)
|
||||
IF(BUILD_PYBULLET_ENET)
|
||||
ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET)
|
||||
ENDIF(BUILD_PYBULLET_ENET)
|
||||
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
ADD_DEFINITIONS(${OSDEF})
|
||||
ENDIF(BUILD_PYBULLET_CLSOCKET)
|
||||
ENDIF(WIN32)
|
||||
|
||||
|
||||
IF(BUILD_PYBULLET_ENET)
|
||||
set(pybullet_SRCS ${pybullet_SRCS}
|
||||
../../examples/SharedMemory/PhysicsClientUDP.cpp
|
||||
../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientUDP.h
|
||||
../../examples/SharedMemory/PhysicsClientUDP_C_API.h
|
||||
../../examples/ThirdPartyLibs/enet/win32.c
|
||||
../../examples/ThirdPartyLibs/enet/unix.c
|
||||
../../examples/ThirdPartyLibs/enet/callbacks.c
|
||||
../../examples/ThirdPartyLibs/enet/compress.c
|
||||
../../examples/ThirdPartyLibs/enet/host.c
|
||||
../../examples/ThirdPartyLibs/enet/list.c
|
||||
../../examples/ThirdPartyLibs/enet/packet.c
|
||||
../../examples/ThirdPartyLibs/enet/peer.c
|
||||
../../examples/ThirdPartyLibs/enet/protocol.c
|
||||
)
|
||||
ENDIF(BUILD_PYBULLET_ENET)
|
||||
|
||||
IF(BUILD_PYBULLET_CLSOCKET)
|
||||
set(pybullet_SRCS ${pybullet_SRCS}
|
||||
../../examples/SharedMemory/PhysicsClientTCP.cpp
|
||||
../../examples/SharedMemory/PhysicsClientTCP.h
|
||||
../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp
|
||||
../../examples/SharedMemory/PhysicsClientTCP_C_API.h
|
||||
../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp
|
||||
../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp
|
||||
../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp
|
||||
)
|
||||
ENDIF()
|
||||
|
||||
ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS})
|
||||
|
||||
@@ -170,35 +26,41 @@ SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(pybullet PROPERTIES DEBUG_POSTFIX "_d")
|
||||
|
||||
|
||||
IF(WIN32)
|
||||
IF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
|
||||
TARGET_LINK_LIBRARIES(pybullet ws2_32 )
|
||||
ENDIF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
|
||||
IF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
|
||||
TARGET_LINK_LIBRARIES(pybullet ws2_32)
|
||||
ENDIF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
|
||||
|
||||
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".pyd" )
|
||||
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".pyd")
|
||||
ENDIF(WIN32)
|
||||
|
||||
IF (APPLE)
|
||||
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".so" )
|
||||
IF(APPLE)
|
||||
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".so")
|
||||
ENDIF()
|
||||
|
||||
TARGET_LINK_LIBRARIES(
|
||||
pybullet
|
||||
bullet_c_api
|
||||
)
|
||||
|
||||
|
||||
TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common)
|
||||
|
||||
IF (WIN32)
|
||||
IF(WIN32)
|
||||
TARGET_LINK_LIBRARIES(pybullet ${PYTHON_LIBRARIES})
|
||||
ELSEIF (APPLE)
|
||||
SET_TARGET_PROPERTIES(pybullet PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")
|
||||
ENDIF ()
|
||||
ELSEIF(APPLE)
|
||||
SET_TARGET_PROPERTIES(
|
||||
pybullet
|
||||
PROPERTIES LINK_FLAGS "-undefined dynamic_lookup"
|
||||
)
|
||||
ENDIF()
|
||||
# else Linux: dont link
|
||||
|
||||
IF(WIN32)
|
||||
SET(PYTHON_SITE_PACKAGES Lib/site-packages CACHE PATH "Python install path")
|
||||
ELSE()
|
||||
SET(PYTHON_SITE_PACKAGES lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages CACHE PATH "Python install path")
|
||||
SET(
|
||||
PYTHON_SITE_PACKAGES
|
||||
lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages
|
||||
CACHE PATH "Python install path"
|
||||
)
|
||||
ENDIF()
|
||||
|
||||
INSTALL(TARGETS pybullet DESTINATION ${PYTHON_SITE_PACKAGES})
|
||||
|
||||
|
||||
@@ -1,128 +1,128 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
p.connect(p.GUI)
|
||||
#don't create a ground plane, to allow for gaps etc
|
||||
p.resetSimulation()
|
||||
#p.createCollisionShape(p.GEOM_PLANE)
|
||||
#p.createMultiBody(0,0)
|
||||
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
|
||||
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
||||
|
||||
sphereRadius = 0.05
|
||||
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
|
||||
|
||||
#a few different ways to create a mesh:
|
||||
|
||||
#convex mesh from obj
|
||||
stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
|
||||
|
||||
boxHalfLength = 0.5
|
||||
boxHalfWidth = 2.5
|
||||
boxHalfHeight = 0.1
|
||||
segmentLength = 5
|
||||
|
||||
colBoxId = p.createCollisionShape(p.GEOM_BOX,
|
||||
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
|
||||
|
||||
mass = 1
|
||||
visualShapeId = -1
|
||||
|
||||
segmentStart = 0
|
||||
|
||||
for i in range(segmentLength):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1])
|
||||
segmentStart = segmentStart - 1
|
||||
|
||||
for i in range(segmentLength):
|
||||
height = 0
|
||||
if (i % 2):
|
||||
height = 1
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1 + height])
|
||||
segmentStart = segmentStart - 1
|
||||
|
||||
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
|
||||
|
||||
for i in range(segmentLength):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1])
|
||||
segmentStart = segmentStart - 1
|
||||
if (i % 2):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
|
||||
baseOrientation=baseOrientation)
|
||||
|
||||
for i in range(segmentLength):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1])
|
||||
width = 4
|
||||
for j in range(width):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=stoneId,
|
||||
basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
|
||||
segmentStart = segmentStart - 1
|
||||
|
||||
link_Masses = [1]
|
||||
linkCollisionShapeIndices = [colBoxId]
|
||||
linkVisualShapeIndices = [-1]
|
||||
linkPositions = [[0, 0, 0]]
|
||||
linkOrientations = [[0, 0, 0, 1]]
|
||||
linkInertialFramePositions = [[0, 0, 0]]
|
||||
linkInertialFrameOrientations = [[0, 0, 0, 1]]
|
||||
indices = [0]
|
||||
jointTypes = [p.JOINT_REVOLUTE]
|
||||
axis = [[1, 0, 0]]
|
||||
|
||||
baseOrientation = [0, 0, 0, 1]
|
||||
for i in range(segmentLength):
|
||||
boxId = p.createMultiBody(0,
|
||||
colSphereId,
|
||||
-1, [segmentStart, 0, -0.1],
|
||||
baseOrientation,
|
||||
linkMasses=link_Masses,
|
||||
linkCollisionShapeIndices=linkCollisionShapeIndices,
|
||||
linkVisualShapeIndices=linkVisualShapeIndices,
|
||||
linkPositions=linkPositions,
|
||||
linkOrientations=linkOrientations,
|
||||
linkInertialFramePositions=linkInertialFramePositions,
|
||||
linkInertialFrameOrientations=linkInertialFrameOrientations,
|
||||
linkParentIndices=indices,
|
||||
linkJointTypes=jointTypes,
|
||||
linkJointAxis=axis)
|
||||
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
|
||||
print(p.getNumJoints(boxId))
|
||||
for joint in range(p.getNumJoints(boxId)):
|
||||
targetVelocity = 10
|
||||
if (i % 2):
|
||||
targetVelocity = -10
|
||||
p.setJointMotorControl2(boxId,
|
||||
joint,
|
||||
p.VELOCITY_CONTROL,
|
||||
targetVelocity=targetVelocity,
|
||||
force=100)
|
||||
segmentStart = segmentStart - 1.1
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||||
while (1):
|
||||
camData = p.getDebugVisualizerCamera()
|
||||
viewMat = camData[2]
|
||||
projMat = camData[3]
|
||||
p.getCameraImage(256,
|
||||
256,
|
||||
viewMatrix=viewMat,
|
||||
projectionMatrix=projMat,
|
||||
renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
||||
keys = p.getKeyboardEvents()
|
||||
p.stepSimulation()
|
||||
#print(keys)
|
||||
time.sleep(0.01)
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
p.connect(p.GUI)
|
||||
#don't create a ground plane, to allow for gaps etc
|
||||
p.resetSimulation()
|
||||
#p.createCollisionShape(p.GEOM_PLANE)
|
||||
#p.createMultiBody(0,0)
|
||||
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
|
||||
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
||||
|
||||
sphereRadius = 0.05
|
||||
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
|
||||
|
||||
#a few different ways to create a mesh:
|
||||
|
||||
#convex mesh from obj
|
||||
stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
|
||||
|
||||
boxHalfLength = 0.5
|
||||
boxHalfWidth = 2.5
|
||||
boxHalfHeight = 0.1
|
||||
segmentLength = 5
|
||||
|
||||
colBoxId = p.createCollisionShape(p.GEOM_BOX,
|
||||
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
|
||||
|
||||
mass = 1
|
||||
visualShapeId = -1
|
||||
|
||||
segmentStart = 0
|
||||
|
||||
for i in range(segmentLength):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1])
|
||||
segmentStart = segmentStart - 1
|
||||
|
||||
for i in range(segmentLength):
|
||||
height = 0
|
||||
if (i % 2):
|
||||
height = 1
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1 + height])
|
||||
segmentStart = segmentStart - 1
|
||||
|
||||
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
|
||||
|
||||
for i in range(segmentLength):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1])
|
||||
segmentStart = segmentStart - 1
|
||||
if (i % 2):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
|
||||
baseOrientation=baseOrientation)
|
||||
|
||||
for i in range(segmentLength):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
basePosition=[segmentStart, 0, -0.1])
|
||||
width = 4
|
||||
for j in range(width):
|
||||
p.createMultiBody(baseMass=0,
|
||||
baseCollisionShapeIndex=stoneId,
|
||||
basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
|
||||
segmentStart = segmentStart - 1
|
||||
|
||||
link_Masses = [1]
|
||||
linkCollisionShapeIndices = [colBoxId]
|
||||
linkVisualShapeIndices = [-1]
|
||||
linkPositions = [[0, 0, 0]]
|
||||
linkOrientations = [[0, 0, 0, 1]]
|
||||
linkInertialFramePositions = [[0, 0, 0]]
|
||||
linkInertialFrameOrientations = [[0, 0, 0, 1]]
|
||||
indices = [0]
|
||||
jointTypes = [p.JOINT_REVOLUTE]
|
||||
axis = [[1, 0, 0]]
|
||||
|
||||
baseOrientation = [0, 0, 0, 1]
|
||||
for i in range(segmentLength):
|
||||
boxId = p.createMultiBody(0,
|
||||
colSphereId,
|
||||
-1, [segmentStart, 0, -0.1],
|
||||
baseOrientation,
|
||||
linkMasses=link_Masses,
|
||||
linkCollisionShapeIndices=linkCollisionShapeIndices,
|
||||
linkVisualShapeIndices=linkVisualShapeIndices,
|
||||
linkPositions=linkPositions,
|
||||
linkOrientations=linkOrientations,
|
||||
linkInertialFramePositions=linkInertialFramePositions,
|
||||
linkInertialFrameOrientations=linkInertialFrameOrientations,
|
||||
linkParentIndices=indices,
|
||||
linkJointTypes=jointTypes,
|
||||
linkJointAxis=axis)
|
||||
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
|
||||
print(p.getNumJoints(boxId))
|
||||
for joint in range(p.getNumJoints(boxId)):
|
||||
targetVelocity = 10
|
||||
if (i % 2):
|
||||
targetVelocity = -10
|
||||
p.setJointMotorControl2(boxId,
|
||||
joint,
|
||||
p.VELOCITY_CONTROL,
|
||||
targetVelocity=targetVelocity,
|
||||
force=100)
|
||||
segmentStart = segmentStart - 1.1
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||||
while (1):
|
||||
camData = p.getDebugVisualizerCamera()
|
||||
viewMat = camData[2]
|
||||
projMat = camData[3]
|
||||
p.getCameraImage(256,
|
||||
256,
|
||||
viewMatrix=viewMat,
|
||||
projectionMatrix=projMat,
|
||||
renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
||||
keys = p.getKeyboardEvents()
|
||||
p.stepSimulation()
|
||||
#print(keys)
|
||||
time.sleep(0.01)
|
||||
|
||||
@@ -1,26 +1,26 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.resetSimulation()
|
||||
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
||||
print("load plane.urdf")
|
||||
p.loadURDF("plane.urdf")
|
||||
print("load r2d2.urdf")
|
||||
|
||||
p.loadURDF("r2d2.urdf", 0, 0, 1)
|
||||
print("load kitchen/1.sdf")
|
||||
p.loadSDF("kitchens/1.sdf")
|
||||
print("load 100 times plate.urdf")
|
||||
for i in range(100):
|
||||
p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||||
|
||||
p.stopStateLogging(timinglog)
|
||||
print("stopped state logging")
|
||||
p.getCameraImage(320, 200)
|
||||
|
||||
while (1):
|
||||
p.stepSimulation()
|
||||
import pybullet as p
|
||||
import time
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.resetSimulation()
|
||||
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
||||
print("load plane.urdf")
|
||||
p.loadURDF("plane.urdf")
|
||||
print("load r2d2.urdf")
|
||||
|
||||
p.loadURDF("r2d2.urdf", 0, 0, 1)
|
||||
print("load kitchen/1.sdf")
|
||||
p.loadSDF("kitchens/1.sdf")
|
||||
print("load 100 times plate.urdf")
|
||||
for i in range(100):
|
||||
p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||||
|
||||
p.stopStateLogging(timinglog)
|
||||
print("stopped state logging")
|
||||
p.getCameraImage(320, 200)
|
||||
|
||||
while (1):
|
||||
p.stepSimulation()
|
||||
|
||||
@@ -1,150 +1,150 @@
|
||||
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
|
||||
#otherwise use testrender.py (slower but compatible without numpy)
|
||||
#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
|
||||
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import itertools
|
||||
import subprocess
|
||||
import numpy as np
|
||||
import pybullet
|
||||
from multiprocessing import Process
|
||||
|
||||
camTargetPos = [0, 0, 0]
|
||||
cameraUp = [0, 0, 1]
|
||||
cameraPos = [1, 1, 1]
|
||||
|
||||
pitch = -10.0
|
||||
roll = 0
|
||||
upAxisIndex = 2
|
||||
camDistance = 4
|
||||
pixelWidth = 84 # 320
|
||||
pixelHeight = 84 # 200
|
||||
nearPlane = 0.01
|
||||
farPlane = 100
|
||||
fov = 60
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
class BulletSim():
|
||||
|
||||
def __init__(self, connection_mode, *argv):
|
||||
self.connection_mode = connection_mode
|
||||
self.argv = argv
|
||||
|
||||
def __enter__(self):
|
||||
print("connecting")
|
||||
optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
|
||||
optionstring += ' --window_backend=2 --render_device=0'
|
||||
|
||||
print(self.connection_mode, optionstring, *self.argv)
|
||||
cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
|
||||
if cid < 0:
|
||||
raise ValueError
|
||||
print("connected")
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
|
||||
|
||||
pybullet.resetSimulation()
|
||||
pybullet.loadURDF("plane.urdf", [0, 0, -1])
|
||||
pybullet.loadURDF("r2d2.urdf")
|
||||
pybullet.loadURDF("duck_vhacd.urdf")
|
||||
pybullet.setGravity(0, 0, -10)
|
||||
|
||||
def __exit__(self, *_, **__):
|
||||
pybullet.disconnect()
|
||||
|
||||
|
||||
def test(num_runs=300, shadow=1, log=True, plot=False):
|
||||
if log:
|
||||
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
|
||||
|
||||
if plot:
|
||||
plt.ion()
|
||||
|
||||
img = np.random.rand(200, 320)
|
||||
#img = [tandard_normal((50,100))
|
||||
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
|
||||
ax = plt.gca()
|
||||
|
||||
times = np.zeros(num_runs)
|
||||
yaw_gen = itertools.cycle(range(0, 360, 10))
|
||||
for i, yaw in zip(range(num_runs), yaw_gen):
|
||||
pybullet.stepSimulation()
|
||||
start = time.time()
|
||||
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
|
||||
roll, upAxisIndex)
|
||||
aspect = pixelWidth / pixelHeight
|
||||
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
|
||||
img_arr = pybullet.getCameraImage(pixelWidth,
|
||||
pixelHeight,
|
||||
viewMatrix,
|
||||
projectionMatrix,
|
||||
shadow=shadow,
|
||||
lightDirection=[1, 1, 1],
|
||||
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
|
||||
#renderer=pybullet.ER_TINY_RENDERER)
|
||||
stop = time.time()
|
||||
duration = (stop - start)
|
||||
if (duration):
|
||||
fps = 1. / duration
|
||||
#print("fps=",fps)
|
||||
else:
|
||||
fps = 0
|
||||
#print("fps=",fps)
|
||||
#print("duraction=",duration)
|
||||
#print("fps=",fps)
|
||||
times[i] = fps
|
||||
|
||||
if plot:
|
||||
rgb = img_arr[2]
|
||||
image.set_data(rgb) #np_img_arr)
|
||||
ax.plot([0])
|
||||
#plt.draw()
|
||||
#plt.show()
|
||||
plt.pause(0.01)
|
||||
|
||||
mean_time = float(np.mean(times))
|
||||
print("mean: {0} for {1} runs".format(mean_time, num_runs))
|
||||
print("")
|
||||
if log:
|
||||
pybullet.stopStateLogging(logId)
|
||||
return mean_time
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
res = []
|
||||
|
||||
with BulletSim(pybullet.DIRECT):
|
||||
print("\nTesting DIRECT")
|
||||
mean_time = test(log=False, plot=True)
|
||||
res.append(("tiny", mean_time))
|
||||
|
||||
with BulletSim(pybullet.DIRECT):
|
||||
plugin_fn = os.path.join(
|
||||
pybullet.__file__.split("bullet3")[0],
|
||||
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
|
||||
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
|
||||
if plugin < 0:
|
||||
print("\nPlugin Failed to load!\n")
|
||||
sys.exit()
|
||||
|
||||
print("\nTesting DIRECT+OpenGL")
|
||||
mean_time = test(log=True)
|
||||
res.append(("plugin", mean_time))
|
||||
|
||||
with BulletSim(pybullet.GUI):
|
||||
print("\nTesting GUI")
|
||||
mean_time = test(log=False)
|
||||
res.append(("egl", mean_time))
|
||||
|
||||
print()
|
||||
print("rendertest.py")
|
||||
print("back nenv fps fps_tot")
|
||||
for r in res:
|
||||
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))
|
||||
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
|
||||
#otherwise use testrender.py (slower but compatible without numpy)
|
||||
#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
|
||||
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import itertools
|
||||
import subprocess
|
||||
import numpy as np
|
||||
import pybullet
|
||||
from multiprocessing import Process
|
||||
|
||||
camTargetPos = [0, 0, 0]
|
||||
cameraUp = [0, 0, 1]
|
||||
cameraPos = [1, 1, 1]
|
||||
|
||||
pitch = -10.0
|
||||
roll = 0
|
||||
upAxisIndex = 2
|
||||
camDistance = 4
|
||||
pixelWidth = 84 # 320
|
||||
pixelHeight = 84 # 200
|
||||
nearPlane = 0.01
|
||||
farPlane = 100
|
||||
fov = 60
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
class BulletSim():
|
||||
|
||||
def __init__(self, connection_mode, *argv):
|
||||
self.connection_mode = connection_mode
|
||||
self.argv = argv
|
||||
|
||||
def __enter__(self):
|
||||
print("connecting")
|
||||
optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
|
||||
optionstring += ' --window_backend=2 --render_device=0'
|
||||
|
||||
print(self.connection_mode, optionstring, *self.argv)
|
||||
cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
|
||||
if cid < 0:
|
||||
raise ValueError
|
||||
print("connected")
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
|
||||
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
|
||||
|
||||
pybullet.resetSimulation()
|
||||
pybullet.loadURDF("plane.urdf", [0, 0, -1])
|
||||
pybullet.loadURDF("r2d2.urdf")
|
||||
pybullet.loadURDF("duck_vhacd.urdf")
|
||||
pybullet.setGravity(0, 0, -10)
|
||||
|
||||
def __exit__(self, *_, **__):
|
||||
pybullet.disconnect()
|
||||
|
||||
|
||||
def test(num_runs=300, shadow=1, log=True, plot=False):
|
||||
if log:
|
||||
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
|
||||
|
||||
if plot:
|
||||
plt.ion()
|
||||
|
||||
img = np.random.rand(200, 320)
|
||||
#img = [tandard_normal((50,100))
|
||||
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
|
||||
ax = plt.gca()
|
||||
|
||||
times = np.zeros(num_runs)
|
||||
yaw_gen = itertools.cycle(range(0, 360, 10))
|
||||
for i, yaw in zip(range(num_runs), yaw_gen):
|
||||
pybullet.stepSimulation()
|
||||
start = time.time()
|
||||
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
|
||||
roll, upAxisIndex)
|
||||
aspect = pixelWidth / pixelHeight
|
||||
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
|
||||
img_arr = pybullet.getCameraImage(pixelWidth,
|
||||
pixelHeight,
|
||||
viewMatrix,
|
||||
projectionMatrix,
|
||||
shadow=shadow,
|
||||
lightDirection=[1, 1, 1],
|
||||
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
|
||||
#renderer=pybullet.ER_TINY_RENDERER)
|
||||
stop = time.time()
|
||||
duration = (stop - start)
|
||||
if (duration):
|
||||
fps = 1. / duration
|
||||
#print("fps=",fps)
|
||||
else:
|
||||
fps = 0
|
||||
#print("fps=",fps)
|
||||
#print("duraction=",duration)
|
||||
#print("fps=",fps)
|
||||
times[i] = fps
|
||||
|
||||
if plot:
|
||||
rgb = img_arr[2]
|
||||
image.set_data(rgb) #np_img_arr)
|
||||
ax.plot([0])
|
||||
#plt.draw()
|
||||
#plt.show()
|
||||
plt.pause(0.01)
|
||||
|
||||
mean_time = float(np.mean(times))
|
||||
print("mean: {0} for {1} runs".format(mean_time, num_runs))
|
||||
print("")
|
||||
if log:
|
||||
pybullet.stopStateLogging(logId)
|
||||
return mean_time
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
res = []
|
||||
|
||||
with BulletSim(pybullet.DIRECT):
|
||||
print("\nTesting DIRECT")
|
||||
mean_time = test(log=False, plot=True)
|
||||
res.append(("tiny", mean_time))
|
||||
|
||||
with BulletSim(pybullet.DIRECT):
|
||||
plugin_fn = os.path.join(
|
||||
pybullet.__file__.split("bullet3")[0],
|
||||
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
|
||||
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
|
||||
if plugin < 0:
|
||||
print("\nPlugin Failed to load!\n")
|
||||
sys.exit()
|
||||
|
||||
print("\nTesting DIRECT+OpenGL")
|
||||
mean_time = test(log=True)
|
||||
res.append(("plugin", mean_time))
|
||||
|
||||
with BulletSim(pybullet.GUI):
|
||||
print("\nTesting GUI")
|
||||
mean_time = test(log=False)
|
||||
res.append(("egl", mean_time))
|
||||
|
||||
print()
|
||||
print("rendertest.py")
|
||||
print("back nenv fps fps_tot")
|
||||
for r in res:
|
||||
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))
|
||||
|
||||
@@ -1,110 +1,110 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
plane = p.loadURDF("plane.urdf")
|
||||
p.setGravity(0, 0, -9.8)
|
||||
p.setTimeStep(1. / 500)
|
||||
#p.setDefaultContactERP(0)
|
||||
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
||||
urdfFlags = p.URDF_USE_SELF_COLLISION
|
||||
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
||||
flags=urdfFlags,
|
||||
useFixedBase=False)
|
||||
|
||||
#enable collision between lower legs
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
print(p.getJointInfo(quadruped, j))
|
||||
|
||||
#2,5,8 and 11 are the lower legs
|
||||
lower_legs = [2, 5, 8, 11]
|
||||
for l0 in lower_legs:
|
||||
for l1 in lower_legs:
|
||||
if (l1 > l0):
|
||||
enableCollision = 1
|
||||
print("collision for pair", l0, l1,
|
||||
p.getJointInfo(quadruped, l0)[12],
|
||||
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
||||
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
||||
|
||||
jointIds = []
|
||||
paramIds = []
|
||||
jointOffsets = []
|
||||
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
||||
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
for i in range(4):
|
||||
jointOffsets.append(0)
|
||||
jointOffsets.append(-0.7)
|
||||
jointOffsets.append(0.7)
|
||||
|
||||
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
jointIds.append(j)
|
||||
|
||||
p.getCameraImage(480, 320)
|
||||
p.setRealTimeSimulation(0)
|
||||
|
||||
joints = []
|
||||
|
||||
with open("data1.txt", "r") as filestream:
|
||||
for line in filestream:
|
||||
print("line=", line)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
currentline = line.split(",")
|
||||
#print (currentline)
|
||||
#print("-----")
|
||||
frame = currentline[0]
|
||||
t = currentline[1]
|
||||
#print("frame[",frame,"]")
|
||||
joints = currentline[2:14]
|
||||
#print("joints=",joints)
|
||||
for j in range(12):
|
||||
targetPos = float(joints[j])
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[j],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[j] * targetPos + jointOffsets[j],
|
||||
force=maxForce)
|
||||
p.stepSimulation()
|
||||
for lower_leg in lower_legs:
|
||||
#print("points for ", quadruped, " link: ", lower_leg)
|
||||
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
||||
#print("num points=",len(pts))
|
||||
#for pt in pts:
|
||||
# print(pt[9])
|
||||
time.sleep(1. / 500.)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
js = p.getJointState(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
paramIds.append(
|
||||
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
||||
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
||||
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
while (1):
|
||||
|
||||
for i in range(len(paramIds)):
|
||||
c = paramIds[i]
|
||||
targetPos = p.readUserDebugParameter(c)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[i],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[i] * targetPos + jointOffsets[i],
|
||||
force=maxForce)
|
||||
import pybullet as p
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
plane = p.loadURDF("plane.urdf")
|
||||
p.setGravity(0, 0, -9.8)
|
||||
p.setTimeStep(1. / 500)
|
||||
#p.setDefaultContactERP(0)
|
||||
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
||||
urdfFlags = p.URDF_USE_SELF_COLLISION
|
||||
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
||||
flags=urdfFlags,
|
||||
useFixedBase=False)
|
||||
|
||||
#enable collision between lower legs
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
print(p.getJointInfo(quadruped, j))
|
||||
|
||||
#2,5,8 and 11 are the lower legs
|
||||
lower_legs = [2, 5, 8, 11]
|
||||
for l0 in lower_legs:
|
||||
for l1 in lower_legs:
|
||||
if (l1 > l0):
|
||||
enableCollision = 1
|
||||
print("collision for pair", l0, l1,
|
||||
p.getJointInfo(quadruped, l0)[12],
|
||||
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
||||
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
||||
|
||||
jointIds = []
|
||||
paramIds = []
|
||||
jointOffsets = []
|
||||
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
||||
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
for i in range(4):
|
||||
jointOffsets.append(0)
|
||||
jointOffsets.append(-0.7)
|
||||
jointOffsets.append(0.7)
|
||||
|
||||
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
jointIds.append(j)
|
||||
|
||||
p.getCameraImage(480, 320)
|
||||
p.setRealTimeSimulation(0)
|
||||
|
||||
joints = []
|
||||
|
||||
with open("data1.txt", "r") as filestream:
|
||||
for line in filestream:
|
||||
print("line=", line)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
currentline = line.split(",")
|
||||
#print (currentline)
|
||||
#print("-----")
|
||||
frame = currentline[0]
|
||||
t = currentline[1]
|
||||
#print("frame[",frame,"]")
|
||||
joints = currentline[2:14]
|
||||
#print("joints=",joints)
|
||||
for j in range(12):
|
||||
targetPos = float(joints[j])
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[j],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[j] * targetPos + jointOffsets[j],
|
||||
force=maxForce)
|
||||
p.stepSimulation()
|
||||
for lower_leg in lower_legs:
|
||||
#print("points for ", quadruped, " link: ", lower_leg)
|
||||
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
||||
#print("num points=",len(pts))
|
||||
#for pt in pts:
|
||||
# print(pt[9])
|
||||
time.sleep(1. / 500.)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
js = p.getJointState(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
paramIds.append(
|
||||
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
||||
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
||||
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
while (1):
|
||||
|
||||
for i in range(len(paramIds)):
|
||||
c = paramIds[i]
|
||||
targetPos = p.readUserDebugParameter(c)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[i],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[i] * targetPos + jointOffsets[i],
|
||||
force=maxForce)
|
||||
|
||||
10259
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj
Normal file
10259
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj
Normal file
File diff suppressed because it is too large
Load Diff
37168
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj
Normal file
37168
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,480 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="mini_cheetah" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<link name="body">
|
||||
<inertial>
|
||||
<mass value="3.3"/>
|
||||
<origin xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.362030" iyz="0" izz="0.042673"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_body.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_body.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--!!!!!!!!!!!! Front Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
|
||||
<joint name="torso_to_abduct_fr_j" type="continuous">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 -0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_fr"/>
|
||||
</joint>
|
||||
<link name="abduct_fr">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0 1.5708" xyz="-0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_fr_to_thigh_fr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
|
||||
<parent link="abduct_fr"/>
|
||||
<child link="thigh_fr"/>
|
||||
</joint>
|
||||
<link name="thigh_fr">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_fr_to_knee_fr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_fr"/>
|
||||
<child link="shank_fr"/>
|
||||
</joint>
|
||||
<link name="shank_fr">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<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"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_fl"/>
|
||||
</joint>
|
||||
<link name="abduct_fl">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0. 0. -1.5708" xyz="-0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_fl_to_thigh_fl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
|
||||
<parent link="abduct_fl"/>
|
||||
<child link="thigh_fl"/>
|
||||
</joint>
|
||||
<link name="thigh_fl">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_fl_to_knee_fl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_fl"/>
|
||||
<child link="shank_fl"/>
|
||||
</joint>
|
||||
<link name="shank_fl">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<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">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="-0.19 -0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_hr"/>
|
||||
</joint>
|
||||
<link name="abduct_hr">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0.0 1.5708" xyz="0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 1.5708" xyz="0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_hr_to_thigh_hr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
|
||||
<parent link="abduct_hr"/>
|
||||
<child link="thigh_hr"/>
|
||||
</joint>
|
||||
<link name="thigh_hr">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_hr_to_knee_hr_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_hr"/>
|
||||
<child link="shank_hr"/>
|
||||
</joint>
|
||||
<link name="shank_hr">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<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">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="-0.19 0.049 0.0"/>
|
||||
<parent link="body"/>
|
||||
<child link="abduct_hl"/>
|
||||
</joint>
|
||||
<link name="abduct_hl">
|
||||
<inertial>
|
||||
<mass value="0.54"/>
|
||||
<origin xyz="0.0 0.036 0."/>
|
||||
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
|
||||
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0.0 -1.5708" xyz="0.055 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_abad.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="3.141592 0 -1.5708" xyz="0.055 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="abduct_hl_to_thigh_hl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
|
||||
<parent link="abduct_hl"/>
|
||||
<child link="thigh_hl"/>
|
||||
</joint>
|
||||
<link name="thigh_hl">
|
||||
<inertial>
|
||||
<mass value="0.634"/>
|
||||
<origin xyz="0.0 0.016 -0.02"/>
|
||||
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
|
||||
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_upper_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="thigh_hl_to_knee_hl_j" type="continuous">
|
||||
<axis xyz="0 -1 0"/>
|
||||
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
|
||||
<parent link="thigh_hl"/>
|
||||
<child link="shank_hl"/>
|
||||
</joint>
|
||||
<link name="shank_hl">
|
||||
<inertial>
|
||||
<mass value="0.064"/>
|
||||
<origin xyz="0.0 0.0 -0.209"/>
|
||||
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mini_lower_link.obj"/>
|
||||
</geometry>
|
||||
<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>
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.13"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.019 -0.019 -0.1502"/>
|
||||
<geometry>
|
||||
<mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.038 0.038 0.305"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.081"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0125"/>
|
||||
<geometry>
|
||||
<mesh filename="d435i.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.09 0.025 0.025"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.13"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
|
||||
<geometry>
|
||||
<mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.074 0.009 0.138"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@@ -1,22 +0,0 @@
|
||||
import pybullet as p
|
||||
cin = p.connect(p.SHARED_MEMORY)
|
||||
if (cin < 0):
|
||||
cin = p.connect(p.GUI)
|
||||
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("quadruped/microtaur/microtaur.urdf", 0.858173,-0.698485,0.227967,-0.002864,0.000163,0.951778,0.306776)]
|
||||
ob = objects[0]
|
||||
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.568555, 0.000000, -2.177277, 1.570089, 0.000000, -2.184705, 1.570229, 0.000000, -2.182261, 1.570008, 0.000000, -2.184197, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, -1.569978, 0.000000, 2.184092, -1.569669, 0.000000, 2.186906, -1.570584, 0.000000, 2.181503, -1.568404, 0.000000, 2.178427 ]
|
||||
for jointIndex in range (p.getNumJoints(ob)):
|
||||
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
cid0 = p.createConstraint(1,35,1,32,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid0,maxForce=1000.000000)
|
||||
cid1 = p.createConstraint(1,7,1,10,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid1,maxForce=1000.000000)
|
||||
cid2 = p.createConstraint(1,41,1,38,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid2,maxForce=1000.000000)
|
||||
cid3 = p.createConstraint(1,13,1,16,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid3,maxForce=1000.000000)
|
||||
p.setGravity(0.000000,0.000000,-10.000000)
|
||||
p.stepSimulation()
|
||||
p.disconnect()
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="xavier">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.64"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.045 -0.045 -0.018"/>
|
||||
<geometry>
|
||||
<mesh filename="xavier.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.092 0.105 0.045"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
Binary file not shown.
@@ -1,29 +0,0 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="0.3"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.082"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0.011 0"/>
|
||||
<geometry>
|
||||
<mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba=".3 .3 .3 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.024 0.047 0.034"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@@ -1 +1 @@
|
||||
from . import *
|
||||
from . import *
|
||||
|
||||
@@ -1,114 +1,114 @@
|
||||
import pybullet as p
|
||||
import pybullet_data as pd
|
||||
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
p.setAdditionalSearchPath(pd.getDataPath())
|
||||
|
||||
plane = p.loadURDF("plane.urdf")
|
||||
p.setGravity(0, 0, -9.8)
|
||||
p.setTimeStep(1. / 500)
|
||||
#p.setDefaultContactERP(0)
|
||||
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
||||
urdfFlags = p.URDF_USE_SELF_COLLISION
|
||||
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
||||
flags=urdfFlags,
|
||||
useFixedBase=False)
|
||||
|
||||
#enable collision between lower legs
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
print(p.getJointInfo(quadruped, j))
|
||||
|
||||
#2,5,8 and 11 are the lower legs
|
||||
lower_legs = [2, 5, 8, 11]
|
||||
for l0 in lower_legs:
|
||||
for l1 in lower_legs:
|
||||
if (l1 > l0):
|
||||
enableCollision = 1
|
||||
print("collision for pair", l0, l1,
|
||||
p.getJointInfo(quadruped, l0)[12],
|
||||
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
||||
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
||||
|
||||
jointIds = []
|
||||
paramIds = []
|
||||
jointOffsets = []
|
||||
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
||||
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
for i in range(4):
|
||||
jointOffsets.append(0)
|
||||
jointOffsets.append(-0.7)
|
||||
jointOffsets.append(0.7)
|
||||
|
||||
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
jointIds.append(j)
|
||||
|
||||
p.getCameraImage(480, 320)
|
||||
p.setRealTimeSimulation(0)
|
||||
|
||||
joints = []
|
||||
|
||||
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
|
||||
for line in filestream:
|
||||
print("line=", line)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
currentline = line.split(",")
|
||||
#print (currentline)
|
||||
#print("-----")
|
||||
frame = currentline[0]
|
||||
t = currentline[1]
|
||||
#print("frame[",frame,"]")
|
||||
joints = currentline[2:14]
|
||||
#print("joints=",joints)
|
||||
for j in range(12):
|
||||
targetPos = float(joints[j])
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[j],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[j] * targetPos + jointOffsets[j],
|
||||
force=maxForce)
|
||||
p.stepSimulation()
|
||||
for lower_leg in lower_legs:
|
||||
#print("points for ", quadruped, " link: ", lower_leg)
|
||||
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
||||
#print("num points=",len(pts))
|
||||
#for pt in pts:
|
||||
# print(pt[9])
|
||||
time.sleep(1. / 500.)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
js = p.getJointState(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
paramIds.append(
|
||||
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
||||
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
||||
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
while (1):
|
||||
|
||||
for i in range(len(paramIds)):
|
||||
c = paramIds[i]
|
||||
targetPos = p.readUserDebugParameter(c)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[i],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[i] * targetPos + jointOffsets[i],
|
||||
force=maxForce)
|
||||
import pybullet as p
|
||||
import pybullet_data as pd
|
||||
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
p.setAdditionalSearchPath(pd.getDataPath())
|
||||
|
||||
plane = p.loadURDF("plane.urdf")
|
||||
p.setGravity(0, 0, -9.8)
|
||||
p.setTimeStep(1. / 500)
|
||||
#p.setDefaultContactERP(0)
|
||||
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
|
||||
urdfFlags = p.URDF_USE_SELF_COLLISION
|
||||
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
|
||||
flags=urdfFlags,
|
||||
useFixedBase=False)
|
||||
|
||||
#enable collision between lower legs
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
print(p.getJointInfo(quadruped, j))
|
||||
|
||||
#2,5,8 and 11 are the lower legs
|
||||
lower_legs = [2, 5, 8, 11]
|
||||
for l0 in lower_legs:
|
||||
for l1 in lower_legs:
|
||||
if (l1 > l0):
|
||||
enableCollision = 1
|
||||
print("collision for pair", l0, l1,
|
||||
p.getJointInfo(quadruped, l0)[12],
|
||||
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
|
||||
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
|
||||
|
||||
jointIds = []
|
||||
paramIds = []
|
||||
jointOffsets = []
|
||||
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
|
||||
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
for i in range(4):
|
||||
jointOffsets.append(0)
|
||||
jointOffsets.append(-0.7)
|
||||
jointOffsets.append(0.7)
|
||||
|
||||
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
jointIds.append(j)
|
||||
|
||||
p.getCameraImage(480, 320)
|
||||
p.setRealTimeSimulation(0)
|
||||
|
||||
joints = []
|
||||
|
||||
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
|
||||
for line in filestream:
|
||||
print("line=", line)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
currentline = line.split(",")
|
||||
#print (currentline)
|
||||
#print("-----")
|
||||
frame = currentline[0]
|
||||
t = currentline[1]
|
||||
#print("frame[",frame,"]")
|
||||
joints = currentline[2:14]
|
||||
#print("joints=",joints)
|
||||
for j in range(12):
|
||||
targetPos = float(joints[j])
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[j],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[j] * targetPos + jointOffsets[j],
|
||||
force=maxForce)
|
||||
p.stepSimulation()
|
||||
for lower_leg in lower_legs:
|
||||
#print("points for ", quadruped, " link: ", lower_leg)
|
||||
pts = p.getContactPoints(quadruped, -1, lower_leg)
|
||||
#print("num points=",len(pts))
|
||||
#for pt in pts:
|
||||
# print(pt[9])
|
||||
time.sleep(1. / 500.)
|
||||
|
||||
for j in range(p.getNumJoints(quadruped)):
|
||||
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
|
||||
info = p.getJointInfo(quadruped, j)
|
||||
js = p.getJointState(quadruped, j)
|
||||
#print(info)
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
|
||||
paramIds.append(
|
||||
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
|
||||
(js[0] - jointOffsets[j]) / jointDirections[j]))
|
||||
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
while (1):
|
||||
|
||||
for i in range(len(paramIds)):
|
||||
c = paramIds[i]
|
||||
targetPos = p.readUserDebugParameter(c)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
p.setJointMotorControl2(quadruped,
|
||||
jointIds[i],
|
||||
p.POSITION_CONTROL,
|
||||
jointDirections[i] * targetPos + jointOffsets[i],
|
||||
force=maxForce)
|
||||
|
||||
@@ -1,467 +0,0 @@
|
||||
import pybullet as p
|
||||
import pybullet_data as pd
|
||||
|
||||
import time
|
||||
import math
|
||||
|
||||
|
||||
def drawInertiaBox(parentUid, parentLinkIndex, color):
|
||||
return
|
||||
dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
|
||||
mass = dyn[0]
|
||||
frictionCoeff = dyn[1]
|
||||
inertia = dyn[2]
|
||||
if (mass > 0):
|
||||
Ixx = inertia[0]
|
||||
Iyy = inertia[1]
|
||||
Izz = inertia[2]
|
||||
boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
|
||||
boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
|
||||
boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
|
||||
|
||||
halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
|
||||
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
|
||||
[-halfExtents[0], halfExtents[1], halfExtents[2]],
|
||||
[halfExtents[0], -halfExtents[1], halfExtents[2]],
|
||||
[-halfExtents[0], -halfExtents[1], halfExtents[2]],
|
||||
[halfExtents[0], halfExtents[1], -halfExtents[2]],
|
||||
[-halfExtents[0], halfExtents[1], -halfExtents[2]],
|
||||
[halfExtents[0], -halfExtents[1], -halfExtents[2]],
|
||||
[-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
|
||||
|
||||
p.addUserDebugLine(pts[0],
|
||||
pts[1],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[1],
|
||||
pts[3],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[3],
|
||||
pts[2],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[2],
|
||||
pts[0],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
|
||||
p.addUserDebugLine(pts[0],
|
||||
pts[4],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[1],
|
||||
pts[5],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[2],
|
||||
pts[6],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[3],
|
||||
pts[7],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
|
||||
p.addUserDebugLine(pts[4 + 0],
|
||||
pts[4 + 1],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[4 + 1],
|
||||
pts[4 + 3],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[4 + 3],
|
||||
pts[4 + 2],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
p.addUserDebugLine(pts[4 + 2],
|
||||
pts[4 + 0],
|
||||
color,
|
||||
1,
|
||||
parentObjectUniqueId=parentUid,
|
||||
parentLinkIndex=parentLinkIndex)
|
||||
|
||||
|
||||
toeConstraint = True
|
||||
useMaximalCoordinates = False
|
||||
useRealTime = 1
|
||||
|
||||
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
|
||||
fixedTimeStep = 1. / 100
|
||||
numSolverIterations = 50
|
||||
|
||||
if (useMaximalCoordinates):
|
||||
fixedTimeStep = 1. / 500
|
||||
numSolverIterations = 200
|
||||
|
||||
speed = 10
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kneeFrictionForce = 0
|
||||
kp = 1
|
||||
kd = .5
|
||||
maxKneeForce = 1000
|
||||
|
||||
physId = p.connect(p.SHARED_MEMORY)
|
||||
|
||||
if (physId < 0):
|
||||
p.connect(p.GUI)
|
||||
#p.resetSimulation()
|
||||
p.setAdditionalSearchPath(pd.getDataPath())
|
||||
angle = 0 # pick in range 0..0.2 radians
|
||||
orn = p.getQuaternionFromEuler([0, angle, 0])
|
||||
p.loadURDF("plane.urdf", [0, 0, 0], orn)
|
||||
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
|
||||
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
|
||||
"genericlogdata.bin",
|
||||
maxLogDof=16,
|
||||
logFlags=p.STATE_LOG_JOINT_TORQUES)
|
||||
p.setTimeOut(4000000)
|
||||
|
||||
p.setGravity(0, 0, 0)
|
||||
p.setTimeStep(fixedTimeStep)
|
||||
|
||||
orn = p.getQuaternionFromEuler([0, 0, 0.4])
|
||||
p.setRealTimeSimulation(0)
|
||||
quadruped = p.loadURDF("quadruped/microtaur/microtaur.urdf", [1, -1, .3],
|
||||
orn,
|
||||
useFixedBase=False,
|
||||
useMaximalCoordinates=useMaximalCoordinates,
|
||||
flags=p.URDF_USE_IMPLICIT_CYLINDER)
|
||||
nJoints = p.getNumJoints(quadruped)
|
||||
|
||||
jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
jointInfo = p.getJointInfo(quadruped, i)
|
||||
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
|
||||
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
|
||||
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
|
||||
knee_front_rightL_joint = jointNameToId['knee_front_rightL_joint']
|
||||
hip_front_rightR_joint = jointNameToId['hip_front_rightR_joint']
|
||||
knee_front_rightR_joint = jointNameToId['knee_front_rightR_joint']
|
||||
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
|
||||
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
|
||||
hip_front_leftR_joint = jointNameToId['hip_front_leftR_joint']
|
||||
knee_front_leftR_joint = jointNameToId['knee_front_leftR_joint']
|
||||
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
|
||||
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
|
||||
knee_front_leftL_joint = jointNameToId['knee_front_leftL_joint']
|
||||
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
|
||||
knee_back_rightR_joint = jointNameToId['knee_back_rightR_joint']
|
||||
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
|
||||
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
|
||||
knee_back_rightL_joint = jointNameToId['knee_back_rightL_joint']
|
||||
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
|
||||
knee_back_leftR_joint = jointNameToId['knee_back_leftR_joint']
|
||||
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
knee_back_leftL_joint = jointNameToId['knee_back_leftL_joint']
|
||||
|
||||
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
|
||||
|
||||
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
|
||||
halfpi = 1.57079632679
|
||||
twopi = 4 * halfpi
|
||||
kneeangle = -2.1834
|
||||
|
||||
dyn = p.getDynamicsInfo(quadruped, -1)
|
||||
mass = dyn[0]
|
||||
friction = dyn[1]
|
||||
localInertiaDiagonal = dyn[2]
|
||||
|
||||
print("localInertiaDiagonal", localInertiaDiagonal)
|
||||
|
||||
#this is a no-op, just to show the API
|
||||
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
|
||||
|
||||
#for i in range (nJoints):
|
||||
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
|
||||
|
||||
drawInertiaBox(quadruped, -1, [1, 0, 0])
|
||||
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
|
||||
|
||||
for i in range(nJoints):
|
||||
drawInertiaBox(quadruped, i, [0, 1, 0])
|
||||
|
||||
if (useMaximalCoordinates):
|
||||
steps = 400
|
||||
for aa in range(steps):
|
||||
p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[0] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[1] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[2] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[3] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[4] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[5] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[6] * halfpi * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[7] * halfpi * float(aa) / steps)
|
||||
|
||||
p.setJointMotorControl2(quadruped, knee_front_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[0] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_front_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[1] * kneeangle * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_leftL_joint, p.POSITION_CONTROL,
|
||||
motordir[2] * kneeangle * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_leftR_joint, p.POSITION_CONTROL,
|
||||
motordir[3] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_front_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[4] * (kneeangle) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_front_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[5] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_rightL_joint, p.POSITION_CONTROL,
|
||||
motordir[6] * (kneeangle + twopi) * float(aa) / steps)
|
||||
p.setJointMotorControl2(quadruped, knee_back_rightR_joint, p.POSITION_CONTROL,
|
||||
motordir[7] * kneeangle * float(aa) / steps)
|
||||
|
||||
p.stepSimulation()
|
||||
#time.sleep(fixedTimeStep)
|
||||
else:
|
||||
|
||||
p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_leftL_joint, motordir[0] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_leftR_joint, motordir[1] * kneeangle)
|
||||
|
||||
p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_leftL_joint, motordir[2] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_leftR_joint, motordir[3] * kneeangle)
|
||||
|
||||
p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_rightL_joint, motordir[4] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
|
||||
p.resetJointState(quadruped, knee_front_rightR_joint, motordir[5] * kneeangle)
|
||||
|
||||
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_rightL_joint, motordir[6] * kneeangle)
|
||||
p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
|
||||
p.resetJointState(quadruped, knee_back_rightR_joint, motordir[7] * kneeangle)
|
||||
|
||||
#p.getNumJoints(1)
|
||||
|
||||
if (toeConstraint):
|
||||
cid = p.createConstraint(quadruped, knee_front_leftR_joint, quadruped, knee_front_leftL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
cid = p.createConstraint(quadruped, knee_front_rightR_joint, quadruped, knee_front_rightL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
cid = p.createConstraint(quadruped, knee_back_leftR_joint, quadruped, knee_back_leftL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
cid = p.createConstraint(quadruped, knee_back_rightR_joint, quadruped, knee_back_rightL_joint,
|
||||
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
|
||||
p.changeConstraint(cid, maxForce=maxKneeForce)
|
||||
|
||||
if (1):
|
||||
p.setJointMotorControl(quadruped, knee_front_leftL_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_front_leftR_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_front_rightL_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_front_rightR_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_rightL_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped, knee_back_rightR_joint, p.VELOCITY_CONTROL, 0,
|
||||
kneeFrictionForce)
|
||||
|
||||
p.setGravity(0, 0, -10)
|
||||
|
||||
legnumbering = [
|
||||
motor_front_leftL_joint, motor_front_leftR_joint, motor_back_leftL_joint,
|
||||
motor_back_leftR_joint, motor_front_rightL_joint, motor_front_rightR_joint,
|
||||
motor_back_rightL_joint, motor_back_rightR_joint
|
||||
]
|
||||
|
||||
for i in range(8):
|
||||
print(legnumbering[i])
|
||||
#use the Minitaur leg numbering
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[0],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[0] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[1],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[1] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[2],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[2] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[3],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[3] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[4],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[4] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[5],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[5] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[6],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[6] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[7],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[7] * 1.57,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
|
||||
t = 0.0
|
||||
t_end = t + 5
|
||||
ref_time = time.time()
|
||||
while (t < t_end):
|
||||
p.setGravity(0, 0, -10)
|
||||
if (useRealTime):
|
||||
t = time.time() - ref_time
|
||||
else:
|
||||
t = t + fixedTimeStep
|
||||
if (useRealTime == 0):
|
||||
p.stepSimulation()
|
||||
time.sleep(fixedTimeStep)
|
||||
|
||||
print("quadruped Id = ")
|
||||
print(quadruped)
|
||||
p.saveWorld("quadru.py")
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
|
||||
|
||||
#jump
|
||||
t = 0.0
|
||||
t_end = t + 100
|
||||
i = 0
|
||||
ref_time = time.time()
|
||||
|
||||
while (1):
|
||||
if (useRealTime):
|
||||
t = time.time() - ref_time
|
||||
else:
|
||||
t = t + fixedTimeStep
|
||||
if (True):
|
||||
|
||||
target = math.sin(t * speed) * jump_amp + 1.57
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[0],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[0] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[1],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[1] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[2],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[2] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[3],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[3] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[4],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[4] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[5],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[5] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[6],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[6] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,
|
||||
jointIndex=legnumbering[7],
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=motordir[7] * target,
|
||||
positionGain=kp,
|
||||
velocityGain=kd,
|
||||
force=maxForce)
|
||||
|
||||
if (useRealTime == 0):
|
||||
p.stepSimulation()
|
||||
time.sleep(fixedTimeStep)
|
||||
@@ -0,0 +1,23 @@
|
||||
import pybullet as p
|
||||
import pybullet_data as pd
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
p.setGravity(0,0,-9.8)
|
||||
p.setAdditionalSearchPath(pd.getDataPath())
|
||||
floor = p.loadURDF("plane.urdf")
|
||||
startPos = [0,0,0.5]
|
||||
robot = p.loadURDF("mini_cheetah/mini_cheetah.urdf",startPos)
|
||||
numJoints = p.getNumJoints(robot)
|
||||
p.changeVisualShape(robot,-1,rgbaColor=[1,1,1,1])
|
||||
for j in range (numJoints):
|
||||
p.changeVisualShape(robot,j,rgbaColor=[1,1,1,1])
|
||||
force=200
|
||||
pos=0
|
||||
p.setJointMotorControl2(robot,j,p.POSITION_CONTROL,pos,force=force)
|
||||
dt = 1./240.
|
||||
p.setTimeStep(dt)
|
||||
while (1):
|
||||
p.stepSimulation()
|
||||
time.sleep(dt)
|
||||
|
||||
Reference in New Issue
Block a user