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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

295
examples/InverseKinematics/InverseKinematicsExample.cpp
View File

@@ -14,38 +14,32 @@
#include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h"
#define RADIAN(X) ((X)*RadiansToDegrees)
#define RADIAN(X) ((X)*RadiansToDegrees)
#define MAX_NUM_NODE 1000
#define MAX_NUM_THETA 1000
#define MAX_NUM_EFFECT 100
#define MAX_NUM_NODE 1000
#define MAX_NUM_THETA 1000
#define MAX_NUM_EFFECT 100
double T = 0;
VectorR3 targetaa[MAX_NUM_EFFECT];
// Make slowdown factor larger to make the simulation take larger, less frequent steps
// Make the constant factor in Tstep larger to make time pass more quickly
//const int SlowdownFactor = 40;
const int SlowdownFactor = 0; // Make higher to take larger steps less frequently
const int SleepsPerStep=SlowdownFactor;
int SleepCounter=0;
const int SlowdownFactor = 0; // Make higher to take larger steps less frequently
const int SleepsPerStep = SlowdownFactor;
int SleepCounter = 0;
//const double Tstep = 0.0005*(double)SlowdownFactor; // Time step
int AxesList; /* list to hold the axes */
int AxesOn; /* ON or OFF */
int AxesList; /* list to hold the axes */
int AxesOn; /* ON or OFF */
float Scale, Scale2; /* scaling factors */
float Scale, Scale2; /* scaling factors */
int JointLimitsOn;
int RestPositionOn;
int UseJacobianTargets1;
int numIteration = 1;
double error = 0.0;
double errorDLS = 0.0;
@@ -60,162 +54,152 @@ extern double Excess[];
extern double dsnorm[];
#endif
void Reset(Tree &tree, Jacobian* m_ikJacobian)
void Reset(Tree& tree, Jacobian* m_ikJacobian)
{
AxesOn = false;
Scale = 1.0;
Scale2 = 0.0; /* because add 1. to it in Display() */
Scale = 1.0;
Scale2 = 0.0; /* because add 1. to it in Display() */
JointLimitsOn = true;
RestPositionOn = false;
UseJacobianTargets1 = false;
tree.Init();
tree.Compute();
m_ikJacobian->Reset();
}
// Update target positions
void UpdateTargets( double T2, Tree & treeY) {
void UpdateTargets(double T2, 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(0.6 * b3Sin(0), 0.6 * b3Cos(0), 0.5 + 0.4 * b3Sin(3 * T));
}
// Does a single update (on one kind of tree)
void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
if ( SleepCounter==0 ) {
void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
{
if (SleepCounter == 0)
{
T += Tstep;
UpdateTargets( T , treeY);
}
UpdateTargets(T, treeY);
}
if ( UseJacobianTargets1 ) {
if (UseJacobianTargets1)
{
jacob->SetJtargetActive();
}
else {
else
{
jacob->SetJendActive();
}
jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
// Calculate the change in theta values
switch (ikMethod) {
// Calculate the change in theta values
switch (ikMethod)
{
case IK_JACOB_TRANS:
jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method
jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK_DLS:
jacob->CalcDeltaThetasDLS(); // Damped least squares method
jacob->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK_DLS_SVD:
jacob->CalcDeltaThetasDLSwithSVD();
break;
case IK_DLS_SVD:
jacob->CalcDeltaThetasDLSwithSVD();
break;
case IK_PURE_PSEUDO:
jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK_SDLS:
jacob->CalcDeltaThetasSDLS(); // Selectively damped least squares method
jacob->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
jacob->ZeroDeltaThetas();
break;
}
if ( SleepCounter==0 ) {
jacob->UpdateThetas(); // Apply the change in the theta values
if (SleepCounter == 0)
{
jacob->UpdateThetas(); // Apply the change in the theta values
jacob->UpdatedSClampValue(targetaa);
SleepCounter = SleepsPerStep;
}
else {
else
{
SleepCounter--;
}
}
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class InverseKinematicsExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
CommonGraphicsApp* m_app;
int m_ikMethod;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
b3AlignedObjectArray<int> m_movingInstances;
b3AlignedObjectArray<int> m_movingInstances;
int m_targetInstance;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
InverseKinematicsExample(CommonGraphicsApp* app, int option)
:m_app(app),
m_ikMethod(option),
m_targetInstance(-1)
InverseKinematicsExample(CommonGraphicsApp* app, int option)
: m_app(app),
m_ikMethod(option),
m_targetInstance(-1)
{
m_app->setUpAxis(2);
{
b3Vector3 extents=b3MakeVector3(100,100,100);
extents[m_app->getUpAxis()]=1;
int xres = 20;
int yres = 20;
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);
}
///create some graphics proxy for the tracking target
///the endeffector tries to track it using Inverse Kinematics
{
{
b3Vector3 extents = b3MakeVector3(100, 100, 100);
extents[m_app->getUpAxis()] = 1;
int xres = 20;
int yres = 20;
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);
}
///create some graphics proxy for the tracking target
///the endeffector tries to track it using Inverse Kinematics
{
int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
b3Vector3 pos = b3MakeVector3(1,1,1);
b3Vector3 pos = b3MakeVector3(1, 1, 1);
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);
}
m_app->m_renderer->writeTransforms();
}
virtual ~InverseKinematicsExample()
{
}
}
m_app->m_renderer->writeTransforms();
}
virtual ~InverseKinematicsExample()
{
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
BuildKukaIIWAShape();
m_ikJacobian = new Jacobian(&m_ikTree);
Reset(m_ikTree,m_ikJacobian);
}
virtual void exitPhysics()
{
Reset(m_ikTree, m_ikJacobian);
}
virtual void exitPhysics()
{
delete m_ikJacobian;
m_ikJacobian = 0;
}
}
void BuildKukaIIWAShape();
@@ -225,7 +209,7 @@ public:
b3Quaternion rot(0, 0, 0, 1);
if (axis.length())
{
rot = b3Quaternion (axis, node->theta);
rot = b3Quaternion(axis, node->theta);
}
act.setIdentity();
act.setRotation(rot);
@@ -235,96 +219,92 @@ public:
{
b3Vector3 lineColor = b3MakeVector3(0, 0, 0);
int lineWidth = 2;
if (node != 0) {
// glPushMatrix();
if (node != 0)
{
// glPushMatrix();
b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z);
b3Vector3 color = b3MakeVector3(0, 1, 0);
int pointSize = 10;
m_app->m_renderer->drawPoint(pos, color, 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);
m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(2), b3MakeVector3(0, 0, 1), lineWidth);
b3Vector3 axisLocal = b3MakeVector3(node->v.x, node->v.y, node->v.z);
b3Vector3 axisWorld = tr.getBasis()*axisLocal;
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);
m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(2), b3MakeVector3(0, 0, 1), lineWidth);
m_app->m_renderer->drawLine(pos, pos + 0.1*axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5);
b3Vector3 axisLocal = b3MakeVector3(node->v.x, node->v.y, node->v.z);
b3Vector3 axisWorld = tr.getBasis() * axisLocal;
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) {
if (node->left)
{
b3Transform act;
getLocalTransform(node->left, act);
b3Transform trl = tr*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
MyDrawTree(node->left, trl); // Draw tree of children recursively
}
// glPopMatrix();
if (node->right) {
// glPopMatrix();
if (node->right)
{
b3Transform act;
getLocalTransform(node->right, act);
b3Transform trr = tr*act;
b3Transform trr = tr * act;
m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth);
MyDrawTree(node->right,trr); // Draw right siblings recursively
MyDrawTree(node->right, trr); // Draw right siblings recursively
}
}
}
virtual void stepSimulation(float deltaTime)
{
virtual void stepSimulation(float deltaTime)
{
DoUpdateStep(deltaTime, m_ikTree, m_ikJacobian, m_ikMethod);
}
virtual void renderScene()
{
}
virtual void renderScene()
{
b3Transform act;
getLocalTransform(m_ikTree.GetRoot(), act);
MyDrawTree(m_ikTree.GetRoot(), act);
b3Vector3 pos = b3MakeVector3(targetaa[0].x, targetaa[0].y, targetaa[0].z);
b3Quaternion orn(0, 0, 0, 1);
m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance);
m_app->m_renderer->writeTransforms();
m_app->m_renderer->renderScene();
}
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x, float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 1.3;
float pitch = -13;
float yaw = 120;
float targetPos[3]={-0.35,0.14,0.25};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
float targetPos[3] = {-0.35, 0.14, 0.25};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]);
}
}
};
void InverseKinematicsExample::BuildKukaIIWAShape()
@@ -338,7 +318,7 @@ void InverseKinematicsExample::BuildKukaIIWAShape()
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_ikNodes.resize(8);//7DOF+additional endeffector
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_ikTree.InsertRoot(m_ikNodes[0]);
@@ -363,16 +343,9 @@ void InverseKinematicsExample::BuildKukaIIWAShape()
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)
class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options)
{
return new InverseKinematicsExample(options.m_guiHelper->getAppInterface(), options.m_option);
}

13
examples/InverseKinematics/InverseKinematicsExample.h
View File

@@ -1,8 +1,15 @@
#ifndef INVERSE_KINEMATICSEXAMPLE_H
#define INVERSE_KINEMATICSEXAMPLE_H
enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS , IK_DLS_SVD};
enum Method
{
IK_JACOB_TRANS = 0,
IK_PURE_PSEUDO,
IK_DLS,
IK_SDLS,
IK_DLS_SVD
};
class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options);
class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options);
#endif //INVERSE_KINEMATICSEXAMPLE_H
#endif //INVERSE_KINEMATICSEXAMPLE_H