From a30ff20e6b51fe908f35d2cd2a04889e17d668f0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 30 Aug 2016 14:44:11 -0700 Subject: [PATCH] preparation for KUKA IK tracking example --- examples/ExampleBrowser/CMakeLists.txt | 4 + examples/ExampleBrowser/ExampleEntries.cpp | 2 + .../InverseKinematicsExample.cpp | 10 +- .../RoboticsLearning/IKTrajectoryHelper.cpp | 145 +++++++++++ .../RoboticsLearning/IKTrajectoryHelper.h | 30 +++ .../RoboticsLearning/KukaGraspExample.cpp | 243 ++++++++++++++++++ examples/RoboticsLearning/KukaGraspExample.h | 27 ++ examples/RoboticsLearning/b3RobotSimAPI.cpp | 54 ++++ examples/RoboticsLearning/b3RobotSimAPI.h | 14 + examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 14 +- examples/ThirdPartyLibs/BussIK/Jacobian.h | 6 +- 12 files changed, 535 insertions(+), 15 deletions(-) create mode 100644 examples/RoboticsLearning/IKTrajectoryHelper.cpp create mode 100644 examples/RoboticsLearning/IKTrajectoryHelper.h create mode 100644 examples/RoboticsLearning/KukaGraspExample.cpp create mode 100644 examples/RoboticsLearning/KukaGraspExample.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 38e4175b3..717d57f28 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -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 diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 60c62c43a..97579cbe8 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -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), diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 4c36037a6..388826752 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -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); diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/RoboticsLearning/IKTrajectoryHelper.cpp new file mode 100644 index 000000000..2e97653f6 --- /dev/null +++ b/examples/RoboticsLearning/IKTrajectoryHelper.cpp @@ -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 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;im_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;im_ikNodes[i]->GetTheta(); + } + return true; +} \ No newline at end of file diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/RoboticsLearning/IKTrajectoryHelper.h new file mode 100644 index 000000000..5b436057f --- /dev/null +++ b/examples/RoboticsLearning/IKTrajectoryHelper.h @@ -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 diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp new file mode 100644 index 000000000..52cf68160 --- /dev/null +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -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 + +#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 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;im_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); +} + + + diff --git a/examples/RoboticsLearning/KukaGraspExample.h b/examples/RoboticsLearning/KukaGraspExample.h new file mode 100644 index 000000000..8275adba8 --- /dev/null +++ b/examples/RoboticsLearning/KukaGraspExample.h @@ -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 diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 686eeecdf..e7daae828 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -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 @@ -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 m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray 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(); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 87efcb7dd..9b50e4d17 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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 }; diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 2b0454ea8..bd8f49e67 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -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; diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index b1c683f8a..502ba0413 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -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; }