AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

preparation for KUKA IK tracking example

Browse Source
This commit is contained in:
Erwin Coumans 2016-08-30 14:44:11 -07:00
parent 900fd86d58
commit a30ff20e6b
12 changed files with 535 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/ExampleBrowser/CMakeLists.txt
View File

@ -216,6 +216,10 @@ SET(BulletExampleBrowser_SRCS
../RoboticsLearning/b3RobotSimAPI.h
../RoboticsLearning/R2D2GraspExample.cpp
../RoboticsLearning/R2D2GraspExample.h
../RoboticsLearning/KukaGraspExample.cpp
../RoboticsLearning/KukaGraspExample.h
../RoboticsLearning/IKTrajectoryHelper.cpp
../RoboticsLearning/IKTrajectoryHelper.h
../RenderingExamples/CoordinateSystemDemo.cpp
../RenderingExamples/CoordinateSystemDemo.h
../RenderingExamples/RaytracerSetup.cpp

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -46,6 +46,7 @@
#include "../MultiThreading/MultiThreadingExample.h"
#include "../InverseDynamics/InverseDynamicsExample.h"
#include "../RoboticsLearning/R2D2GraspExample.h"
#include "../RoboticsLearning/KukaGraspExample.h"
#include "../RoboticsLearning/GripperGraspExample.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
@ -260,6 +261,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
ExampleEntry(1,"Kuka IK","Control a Kuka IIWA robot to follow a target using IK. This IK is not setup properly yet.", KukaGraspExampleCreateFunc,0),
ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc),

10
examples/InverseKinematics/InverseKinematicsExample.cpp
View File

@ -21,7 +21,7 @@
#define MAX_NUM_EFFECT 100
double T = 0;
VectorR3 target1[MAX_NUM_EFFECT];
VectorR3 targetaa[MAX_NUM_EFFECT];
@ -85,7 +85,7 @@ void Reset(Tree &tree, Jacobian* m_ikJacobian)
void UpdateTargets( double T2, Tree & treeY) {
double T = T2 / 5.;
target1[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));
}
@ -103,7 +103,7 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
else {
jacob->SetJendActive();
}
jacob->ComputeJacobian(); // Set up Jacobian and deltaS vectors
jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
// Calculate the change in theta values
switch (ikMethod) {
@ -129,7 +129,7 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
if ( SleepCounter==0 ) {
jacob->UpdateThetas(); // Apply the change in the theta values
jacob->UpdatedSClampValue();
jacob->UpdatedSClampValue(targetaa);
SleepCounter = SleepsPerStep;
}
else {
@ -290,7 +290,7 @@ public:
getLocalTransform(m_ikTree.GetRoot(), act);
MyDrawTree(m_ikTree.GetRoot(), act);
b3Vector3 pos = b3MakeVector3(target1[0].x, target1[0].y, target1[0].z);
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);

145
examples/RoboticsLearning/IKTrajectoryHelper.cpp Normal file
View File

@ -0,0 +1,145 @@
#include "IKTrajectoryHelper.h"
#include "BussIK/Node.h"
#include "BussIK/Tree.h"
#include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#define RADIAN(X) ((X)*RadiansToDegrees)
//use BussIK and Reflexxes to convert from Cartesian endeffector future target to
//joint space positions at each real-time (simulation) step
struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
}
};
IKTrajectoryHelper::IKTrajectoryHelper()
{
m_data = new IKTrajectoryHelperInternalData;
}
IKTrajectoryHelper::~IKTrajectoryHelper()
{
delete m_data;
}
void IKTrajectoryHelper::createKukaIIWA()
{
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;
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_data->m_ikNodes.resize(8);//7DOF+additional endeffector
m_data->m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
m_data->m_ikTree.InsertRoot(m_data->m_ikNodes[0]);
m_data->m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[0], m_data->m_ikNodes[1]);
m_data->m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[1], m_data->m_ikNodes[2]);
m_data->m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[2], m_data->m_ikNodes[3]);
m_data->m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[3], m_data->m_ikNodes[4]);
m_data->m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[4], m_data->m_ikNodes[5]);
m_data->m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[5], m_data->m_ikNodes[6]);
m_data->m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[6], m_data->m_ikNodes[7]);
m_data->m_ikJacobian = new Jacobian(&m_data->m_ikTree);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikTree.Init();
m_data->m_ikTree.Compute();
m_data->m_ikJacobian->Reset();
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,
double* q_new, int ikMethod)
{
if (numQ != 7)
{
return false;
}
for (int i=0;i<numQ;i++)
{
m_data->m_ikNodes[i]->SetTheta(q_current[i]);
}
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
m_data->m_ikJacobian->SetJtargetActive();
}
else {
m_data->m_ikJacobian->SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
m_data->m_ikJacobian->ZeroDeltaThetas();
break;
}
m_data->m_ikJacobian->UpdateThetas();
// Apply the change in the theta values
m_data->m_ikJacobian->UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}

30
examples/RoboticsLearning/IKTrajectoryHelper.h Normal file
View File

@ -0,0 +1,30 @@
#ifndef IK_TRAJECTORY_HELPER_H
#define IK_TRAJECTORY_HELPER_H
enum IK2_Method
{
IK2_JACOB_TRANS=0,
IK2_PURE_PSEUDO,
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD
};
class IKTrajectoryHelper
{
struct IKTrajectoryHelperInternalData* m_data;
public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
bool computeIK(const double endEffectorTargetPosition[3],
const double* q_old, int numQ,
double* q_new, int ikMethod);
};
#endif //IK_TRAJECTORY_HELPER_H

243
examples/RoboticsLearning/KukaGraspExample.cpp Normal file
View File

@ -0,0 +1,243 @@
#include "KukaGraspExample.h"
#include "IKTrajectoryHelper.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class KukaGraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
int m_kukaIndex;
IKTrajectoryHelper m_ikHelper;
int m_targetSphereInstance;
b3Vector3 m_targetPos;
double m_time;
int m_options;
b3AlignedObjectArray<int> m_movingInstances;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
KukaGraspExample(GUIHelperInterface* helper, int options)
:m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_kukaIndex(-1),
m_time(0)
{
m_targetPos.setValue(0.5,0,1);
m_app->setUpAxis(2);
}
virtual ~KukaGraspExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
///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);
b3Quaternion orn(0, 0, 0, 1);
b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling);
}
m_app->m_renderer->writeTransforms();
m_ikHelper.createKukaIIWA();
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kuka_iiwa/model.urdf";
args.m_startPosition.setValue(0,0,0);
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
m_kukaIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
/*
int wheelJointIndices[4]={2,3,6,7};
int wheelTargetVelocities[4]={-10,-10,-10,-10};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs);
}
*/
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
args.m_forceOverrideFixedBase = true;
args.m_fileType = B3_SDF_FILE;
args.m_startOrientation = b3Quaternion(0,0,0,1);
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,0));
}
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
m_time+=deltaTime;
m_targetPos.setValue(0.5, 0, 0.5+0.4*b3Sin(3 * m_time));
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
if (numJoints==7)
{
double q_current[7]={0,0,0,0,0,0,0};
b3JointStates jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
//skip the base positions (7 values)
b3Assert(7+numJoints == jointStates.m_numDegreeOfFreedomQ);
for (int i=0;i<numJoints;i++)
{
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
// m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
double q_new[7];
int ikMethod=IK2_SDLS;
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod);
printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
q_new[3],q_new[4],q_new[5],q_new[6]);
//set the
for (int i=0;i<numJoints;i++)
{
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = q_new[i];
t.m_maxTorqueValue = 1000;
t.m_kp= 1;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
b3Quaternion orn(0, 0, 0, 1);
m_app->m_renderer->writeSingleInstanceTransformToCPU(m_targetPos, orn, m_targetSphereInstance);
m_app->m_renderer->writeTransforms();
//draw the end-effector target sphere
//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 resetCamera()
{
float dist = 3;
float pitch = -75;
float yaw = 30;
float targetPos[3]={-0.2,0.8,0.3};
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]);
}
}
};
class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options)
{
return new KukaGraspExample(options.m_guiHelper, options.m_option);
}

27
examples/RoboticsLearning/KukaGraspExample.h Normal file
View File

@ -0,0 +1,27 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2016 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 KUKA_GRASP_EXAMPLE_H
#define KUKA_GRASP_EXAMPLE_H
enum KukaGraspExampleOptions
{
eKUKA_GRASP_DLS_IK=1,
};
class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options);
#endif //KUKA_GRASP_EXAMPLE_H

54
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@ -615,6 +615,60 @@ void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int c
}
}
bool b3RobotSimAPI::getJointStates(int bodyUniqueId, b3JointStates& state)
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
if (statusHandle)
{
double rootInertialFrame[7];
const double* rootLocalInertialFrame;
const double* actualStateQ;
const double* actualStateQdot;
const double* jointReactionForces;
int stat = b3GetStatusActualState(statusHandle,
&state.m_bodyUniqueId,
&state.m_numDegreeOfFreedomQ,
&state.m_numDegreeOfFreedomU,
&rootLocalInertialFrame,
&actualStateQ,
&actualStateQdot,
&jointReactionForces);
if (stat)
{
state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
{
state.m_actualStateQ[i] = actualStateQ[i];
}
for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
{
state.m_actualStateQdot[i] = actualStateQdot[i];
}
int numJoints = getNumJoints(bodyUniqueId);
state.m_jointReactionForces.resize(6*numJoints);
for (int i=0;i<numJoints*6;i++)
{
state.m_jointReactionForces[i] = jointReactionForces[i];
}
return true;
}
//rootInertialFrame,
// &state.m_actualStateQ[0],
// &state.m_actualStateQdot[0],
// &state.m_jointReactionForces[0]);
}
return false;
}
void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
{
b3SharedMemoryStatusHandle statusHandle;

14
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@ -6,6 +6,7 @@
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
@ -17,6 +18,17 @@ enum b3RigidSimFileType
B3_AUTO_DETECT_FILE//todo based on extension
};
struct b3JointStates
{
int m_bodyUniqueId;
int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU;
b3Transform m_rootLocalInertialFrame;
b3AlignedObjectArray<double> m_actualStateQ;
b3AlignedObjectArray<double> m_actualStateQdot;
b3AlignedObjectArray<double> m_jointReactionForces;
};
struct b3RobotSimLoadFileArgs
{
std::string m_fileName;
@ -91,6 +103,8 @@ public:
void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointStates(int bodyUniqueId, b3JointStates& state);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
void stepSimulation();

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -28,6 +28,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_CALCULATE_INVERSE_KINEMATICS,
CMD_MAX_CLIENT_COMMANDS,
CMD_CREATE_JOINT
};

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

@ -36,7 +36,7 @@ using namespace std;
void Arrow(const VectorR3& tail, const VectorR3& head);
//extern RestPositionOn;
extern VectorR3 target1[];
//extern VectorR3 target1[];
// Optimal damping values have to be determined in an ad hoc manner (Yuck!)
const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter)
@ -96,7 +96,7 @@ void Jacobian::Reset()
// Compute the deltaS vector, dS, (the error in end effector positions
// Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian()
void Jacobian::ComputeJacobian(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
@ -104,7 +104,7 @@ void Jacobian::ComputeJacobian()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
temp = targetPos;
@ -418,7 +418,7 @@ void Jacobian::CalcdTClampedFromdS()
}
}
double Jacobian::UpdateErrorArray()
double Jacobian::UpdateErrorArray(VectorR3* targets)
{
double totalError = 0.0;
@ -428,7 +428,7 @@ double Jacobian::UpdateErrorArray()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
temp = targetPos;
temp -= n->GetS();
double err = temp.Norm();
@ -440,7 +440,7 @@ double Jacobian::UpdateErrorArray()
return totalError;
}
void Jacobian::UpdatedSClampValue()
void Jacobian::UpdatedSClampValue(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
@ -448,7 +448,7 @@ void Jacobian::UpdatedSClampValue()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
// While we are at it, also update the clamping values in dSclamp;

6
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@ -55,7 +55,7 @@ class Jacobian {
public:
Jacobian(Tree*);
void ComputeJacobian();
void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
void SetJtargetActive() { Jactive = &Jtarget; }
@ -69,9 +69,9 @@ public:
void CalcDeltaThetasSDLS();
void UpdateThetas();
double UpdateErrorArray(); // Returns sum of errors
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
const VectorRn& GetErrorArray() const { return errorArray; }
void UpdatedSClampValue();
void UpdatedSClampValue(VectorR3* targets);
void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; }
UpdateMode GetCurrentMode() const { return CurrentUpdateMode; }