diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index f5735d0e0..144098f83 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -78,7 +78,6 @@ public: - m_ikHelper.createKukaIIWA(); bool connected = m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d",connected); @@ -114,6 +113,7 @@ public: */ } + if (0) { b3RobotSimLoadFileArgs args(""); args.m_fileName = "kiva_shelf/model.sdf"; @@ -195,11 +195,13 @@ public: ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1]; ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2]; + ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION; + ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0]; ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1]; ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2]; ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3]; - ikargs.m_dt = dt; + ikargs.m_endEffectorLinkIndex = 6; if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) { diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 873ddfc36..1a9d9d2c2 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -480,12 +480,25 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status */ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results) { + btAssert(args.m_endEffectorLinkIndex>=0); + btAssert(args.m_bodyUniqueId>=0); + - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId, args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation,args.m_dt); + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); + if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); + } else + { + b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); + } + b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + + int numPos=0; bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index bb5fe6d41..f46e7ec3e 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -94,15 +94,22 @@ struct b3RobotSimInverseKinematicArgs // int m_numPositions; double m_endEffectorTargetPosition[3]; double m_endEffectorTargetOrientation[4]; - double m_dt; + int m_endEffectorLinkIndex; int m_flags; b3RobotSimInverseKinematicArgs() :m_bodyUniqueId(-1), -// m_currentJointPositions(0), -// m_numPositions(0), + m_endEffectorLinkIndex(-1), m_flags(0) { + m_endEffectorTargetPosition[0]=0; + m_endEffectorTargetPosition[1]=0; + m_endEffectorTargetPosition[2]=0; + + m_endEffectorTargetOrientation[0]=0; + m_endEffectorTargetOrientation[1]=0; + m_endEffectorTargetOrientation[2]=0; + m_endEffectorTargetOrientation[3]=1; } }; diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 3e4537ea9..d2d23708c 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -16,7 +16,7 @@ struct IKTrajectoryHelperInternalData { VectorR3 m_endEffectorTargetPosition; - Tree m_ikTree; + b3AlignedObjectArray m_ikNodes; Jacobian* m_ikJacobian; @@ -37,82 +37,22 @@ IKTrajectoryHelper::~IKTrajectoryHelper() delete m_data; } -bool IKTrajectoryHelper::createFromMultiBody(class btMultiBody* mb) -{ - //todo: implement proper conversion. For now, we only 'detect' a likely KUKA iiwa and hardcode its creation - if (mb->getNumLinks()==7) - { - createKukaIIWA(); - return true; - } - - return false; -} - -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 endEffectorTargetOrientation[4], const double endEffectorWorldPosition[3], const double endEffectorWorldOrientation[4], - const double* q_current, int numQ, - double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt) + const double* q_current, int numQ,int endEffectorIndex, + double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size) { + bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false; + + m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ); + +// Reset(m_ikTree,m_ikJacobian); + + m_data->m_ikJacobian->Reset(); - if (numQ != 7) - { - return false; - } - - for (int i=0;im_ikNodes[i]->SetTheta(q_current[i]); - } bool UseJacobianTargets1 = false; if ( UseJacobianTargets1 ) { @@ -129,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], VectorRn deltaS(3); for (int i = 0; i < 3; ++i) { - deltaS.Set(i,(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])/dt); + deltaS.Set(i,(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])); } // Set one end effector world orientation from Bullet @@ -139,35 +79,49 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], btQuaternion deltaQ = endQ*startQ.inverse(); float angle = deltaQ.getAngle(); btVector3 axis = deltaQ.getAxis(); - float angleDot = angle/dt; + float angleDot = angle; btVector3 angularVel = angleDot*axis.normalize(); for (int i = 0; i < 3; ++i) { deltaR.Set(i,angularVel[i]); } - VectorRn deltaC(6); - for (int i = 0; i < 3; ++i) - { - deltaC.Set(i,deltaS[i]); - deltaC.Set(i+3,deltaR[i]); - } - m_data->m_ikJacobian->SetDeltaS(deltaC); - // Set Jacobian from Bullet body Jacobian - int nRow = m_data->m_ikJacobian->GetNumRows(); - int nCol = m_data->m_ikJacobian->GetNumCols(); - b3Assert(jacobian_size==nRow*nCol); - MatrixRmn completeJacobian(nRow,nCol); - for (int i = 0; i < nRow/2; ++i) + { - for (int j = 0; j < nCol; ++j) - { - completeJacobian.Set(i,j,linear_jacobian[i*nCol+j]); - completeJacobian.Set(i+nRow/2,j,angular_jacobian[i*nCol+j]); - } + + if (useAngularPart) + { + VectorRn deltaC(6); + MatrixRmn completeJacobian(6,numQ); + for (int i = 0; i < 3; ++i) + { + deltaC.Set(i,deltaS[i]); + deltaC.Set(i+3,deltaR[i]); + for (int j = 0; j < numQ; ++j) + { + completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]); + completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]); + } + } + m_data->m_ikJacobian->SetDeltaS(deltaC); + m_data->m_ikJacobian->SetJendTrans(completeJacobian); + } else + { + VectorRn deltaC(3); + MatrixRmn completeJacobian(3,numQ); + for (int i = 0; i < 3; ++i) + { + deltaC.Set(i,deltaS[i]); + for (int j = 0; j < numQ; ++j) + { + completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]); + } + } + m_data->m_ikJacobian->SetDeltaS(deltaC); + m_data->m_ikJacobian->SetJendTrans(completeJacobian); + } } - m_data->m_ikJacobian->SetJendTrans(completeJacobian); // Calculate the change in theta values switch (ikMethod) { @@ -175,6 +129,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method break; case IK2_DLS: + case IK2_VEL_DLS_WITH_ORIENTATION: + case IK2_VEL_DLS: m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method break; case IK2_DLS_SVD: @@ -186,9 +142,6 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], case IK2_SDLS: m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method break; - case IK2_VEL_DLS: - m_data->m_ikJacobian->CalcThetasDotDLS(dt); // Damped least square for velocity IK - break; default: m_data->m_ikJacobian->ZeroDeltaThetas(); break; @@ -206,7 +159,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], for (int i=0;im_ikNodes[i]->GetTheta()*dt + q_current[i]; + q_new[i] = m_data->m_ikJacobian->dTheta[i] + q_current[i]; // Use for position IK //q_new[i] = m_data->m_ikNodes[i]->GetTheta(); diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index ca786a104..5aaba770e 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -8,7 +8,8 @@ enum IK2_Method IK2_DLS, IK2_SDLS , IK2_DLS_SVD, - IK2_VEL_DLS + IK2_VEL_DLS, + IK2_VEL_DLS_WITH_ORIENTATION, }; @@ -20,17 +21,13 @@ public: IKTrajectoryHelper(); virtual ~IKTrajectoryHelper(); - ///todo: replace createKukaIIWA with a generic way of create an IK tree - void createKukaIIWA(); - - bool createFromMultiBody(class btMultiBody* mb); - + bool computeIK(const double endEffectorTargetPosition[3], const double endEffectorTargetOrientation[4], const double endEffectorWorldPosition[3], const double endEffectorWorldOrientation[4], - const double* q_old, int numQ, - double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt); + const double* q_old, int numQ,int endEffectorIndex, + double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size); }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d6e113fe2..61a192fa8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1319,7 +1319,7 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ } ///compute the joint positions to move the end effector to a desired target using inverse kinematics -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt) +b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -1330,12 +1330,32 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS; command->m_updateFlags = 0; command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex; -//todo -// int numJoints = cl->getNumJoints(bodyIndex); -// for (int i = 0; i < numJoints;i++) -// { -// command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; -// } + + return (b3SharedMemoryCommandHandle)command; + +} + +void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_TARGET_POSITION; + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; + + command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; + + +} +void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION; + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; @@ -1345,12 +1365,10 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3]; - command->m_calculateInverseKinematicsArguments.m_dt = dt; - - return (b3SharedMemoryCommandHandle)command; } + int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f3d9acd9c..a57851fd8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -121,8 +121,9 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian); ///compute the joint positions to move the end effector to a desired target using inverse kinematics -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt); - +b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); +void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]); +void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]); int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b12ee1af0..786f68d41 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2551,7 +2551,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED; - InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); + InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); @@ -2565,41 +2565,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm else { IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; - if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody)) - { - m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); - ikHelperPtr = tmpHelper; - } else - { - delete tmpHelper; - } + m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); + ikHelperPtr = tmpHelper; } - //todo: make this generic. Right now, only support/tested KUKA iiwa - int numJoints = 7; - int endEffectorLinkIndex = 6; + int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; - if (ikHelperPtr && bodyHandle->m_multiBody->getNumLinks()==numJoints) + + if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) { + int numJoints1 = bodyHandle->m_multiBody->getNumLinks(); + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*7); + jacobian_linear.resize(3*numDofs); b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*7); + jacobian_angular.resize(3*numDofs); int jacSize = 0; btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - double q_current[7]; + + + btAlignedObjectArray q_current; + q_current.resize(numDofs); if (tree) { jacSize = jacobian_linear.size(); // Set jacobian value int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); - btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); - for (int i = 0; i < num_dofs; i++) + + btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + for (int i = 0; i < numDofs; i++) { q_current[i] = bodyHandle->m_multiBody->getJointPos(i); q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); @@ -2613,24 +2612,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) { tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, num_dofs); - btInverseDynamics::mat3x jac_r(3,num_dofs); + btInverseDynamics::mat3x jac_t(3, numDofs); + btInverseDynamics::mat3x jac_r(3,numDofs); tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t); tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r); for (int i = 0; i < 3; ++i) { - for (int j = 0; j < num_dofs; ++j) + for (int j = 0; j < numDofs; ++j) { - jacobian_linear[i*num_dofs+j] = jac_t(i,j); - jacobian_angular[i*num_dofs+j] = jac_r(i,j); + jacobian_linear[i*numDofs+j] = jac_t(i,j); + jacobian_angular[i*numDofs+j] = jac_r(i,j); } } } } - double q_new[7]; - int ikMethod=IK2_VEL_DLS; + btAlignedObjectArray q_new; + q_new.resize(numDofs); + int ikMethod= (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)? IK2_VEL_DLS_WITH_ORIENTATION : IK2_VEL_DLS; btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldOrientation; @@ -2644,15 +2644,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - q_current, - numJoints, q_new, ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2,clientCmd.m_calculateInverseKinematicsArguments.m_dt); + &q_current[0], + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2); serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i=0;iGetNumEffector(); + m_tree = tree; + m_nEffector = tree->GetNumEffector(); nJoint = tree->GetNumJoint(); - nRow = 2 * 3 * nEffector; // Include both linear and angular part + nRow = 3 * m_nEffector; // Include only the linear part + nCol = nJoint; Jend.SetSize(nRow, nCol); // The Jocobian matrix @@ -77,9 +78,52 @@ Jacobian::Jacobian(Tree* tree) // Used by the Selectively Damped Least Squares Method //dT.SetLength(nRow); - dSclamp.SetLength(nEffector); - errorArray.SetLength(nEffector); - Jnorms.SetSize(nEffector, nCol); // Holds the norms of the active J matrix + dSclamp.SetLength(m_nEffector); + errorArray.SetLength(m_nEffector); + Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix + + Reset(); +} + + +Jacobian::Jacobian(bool useAngularJacobian,int nDof) +{ + + m_tree = 0; + m_nEffector = 1; + + if (useAngularJacobian) + { + nRow = 2 * 3 * m_nEffector; // Include both linear and angular part + } else + { + nRow = 3 * m_nEffector; // Include only the linear part + } + + nCol = nDof; + + Jend.SetSize(nRow, nCol); // The Jocobian matrix + Jend.SetZero(); + Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions + Jtarget.SetZero(); + SetJendActive(); + + U.SetSize(nRow, nRow); // The U matrix for SVD calculations + w .SetLength(Min(nRow, nCol)); + V.SetSize(nCol, nCol); // The V matrix for SVD calculations + + dS.SetLength(nRow); // (Target positions) - (End effector positions) + dTheta.SetLength(nCol); // Changes in joint angles + dPreTheta.SetLength(nCol); + + // Used by Jacobian transpose method & DLS & SDLS + dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta + + // Used by the Selectively Damped Least Squares Method + //dT.SetLength(nRow); + dSclamp.SetLength(m_nEffector); + errorArray.SetLength(m_nEffector); + Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix Reset(); } @@ -98,9 +142,12 @@ void Jacobian::Reset() // Compute the J and K matrices (the Jacobians) void Jacobian::ComputeJacobian(VectorR3* targets) { + if (m_tree==0) + return; + // Traverse tree to find all end effectors VectorR3 temp; - Node* n = tree->GetRoot(); + Node* n = m_tree->GetRoot(); while ( n ) { if ( n->IsEffector() ) { int i = n->GetEffectorNum(); @@ -113,10 +160,10 @@ void Jacobian::ComputeJacobian(VectorR3* targets) // Find all ancestors (they will usually all be joints) // Set the corresponding entries in the Jacobians J, K. - Node* m = tree->GetParent(n); + Node* m = m_tree->GetParent(n); while ( m ) { int j = m->GetJointNum(); - assert ( 0 <=i && iIsFrozen() ) { Jend.SetTriple(i, j, VectorR3::Zero); Jtarget.SetTriple(i, j, VectorR3::Zero); @@ -131,10 +178,10 @@ void Jacobian::ComputeJacobian(VectorR3* targets) temp *= m->GetW(); // cross product with joint rotation axis Jtarget.SetTriple(i, j, temp); } - m = tree->GetParent( m ); + m = m_tree->GetParent( m ); } } - n = tree->GetSuccessor( n ); + n = m_tree->GetSuccessor( n ); } } @@ -156,36 +203,39 @@ void Jacobian::UpdateThetas() { // Traverse the tree to find all joints // Update the joint angles - Node* n = tree->GetRoot(); + Node* n = m_tree->GetRoot(); while ( n ) { if ( n->IsJoint() ) { int i = n->GetJointNum(); n->AddToTheta( dTheta[i] ); } - n = tree->GetSuccessor( n ); + n = m_tree->GetSuccessor( n ); } // Update the positions and rotation axes of all joints/effectors - tree->Compute(); + m_tree->Compute(); } void Jacobian::UpdateThetaDot() { + if (m_tree==0) + return; + // Traverse the tree to find all joints // Update the joint angles - Node* n = tree->GetRoot(); + Node* n = m_tree->GetRoot(); while ( n ) { if ( n->IsJoint() ) { int i = n->GetJointNum(); n->UpdateTheta( dTheta[i] ); } - n = tree->GetSuccessor( n ); + n = m_tree->GetSuccessor( n ); } // Update the positions and rotation axes of all joints/effectors - tree->Compute(); + m_tree->Compute(); } void Jacobian::CalcDeltaThetas() @@ -279,7 +329,7 @@ void Jacobian::CalcDeltaThetasDLS() // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e. // CalcdTClampedFromdS(); - // VectorRn dTextra(3*nEffector); + // VectorRn dTextra(3*m_nEffector); // U.Solve( dT, &dTextra ); // J.MultiplyTranspose( dTextra, dTheta ); @@ -294,31 +344,6 @@ void Jacobian::CalcDeltaThetasDLS() } } -void Jacobian::CalcThetasDotDLS(float dt) -{ - const MatrixRmn& J = ActiveJacobian(); - - MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T) - U.AddToDiagonal( DampingLambdaSq ); - - // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e. - // CalcdTClampedFromdS(); - // VectorRn dTextra(3*nEffector); - // U.Solve( dT, &dTextra ); - // J.MultiplyTranspose( dTextra, dTheta ); - - // Use these two lines for the traditional DLS method - U.Solve( dS, &dT1 ); - J.MultiplyTranspose( dT1, dTheta ); - - // Scale back to not exceed maximum angle changes - double MaxVelDLS = MaxAngleDLS/dt; - double maxChange = dTheta.MaxAbs(); - if ( maxChange>MaxVelDLS ) { - dTheta *= MaxVelDLS/maxChange; - } -} - void Jacobian::CalcDeltaThetasDLSwithSVD() { const MatrixRmn& J = ActiveJacobian(); @@ -366,7 +391,7 @@ void Jacobian::CalcDeltaThetasSDLS() // Calculate response vector dTheta that is the SDLS solution. // Delta target values are the dS values int nRows = J.GetNumRows(); - int numEndEffectors = tree->GetNumEffector(); // Equals the number of rows of J divided by three + int numEndEffectors = m_tree->GetNumEffector(); // Equals the number of rows of J divided by three int nCols = J.GetNumColumns(); dTheta.SetZero(); @@ -478,7 +503,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets) // Traverse tree to find all end effectors VectorR3 temp; - Node* n = tree->GetRoot(); + Node* n = m_tree->GetRoot(); while ( n ) { if ( n->IsEffector() ) { int i = n->GetEffectorNum(); @@ -489,7 +514,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets) errorArray[i] = err; totalError += err; } - n = tree->GetSuccessor( n ); + n = m_tree->GetSuccessor( n ); } return totalError; } @@ -498,7 +523,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets) { // Traverse tree to find all end effectors VectorR3 temp; - Node* n = tree->GetRoot(); + Node* n = m_tree->GetRoot(); while ( n ) { if ( n->IsEffector() ) { int i = n->GetEffectorNum(); @@ -517,7 +542,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets) dSclamp[i] = BaseMaxTargetDist; } } - n = tree->GetSuccessor( n ); + n = m_tree->GetSuccessor( n ); } } diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 38aa475c0..e02a3bee4 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -54,6 +54,7 @@ enum UpdateMode { class Jacobian { public: Jacobian(Tree*); + Jacobian(bool useAngularJacobian, int nDof); void ComputeJacobian(VectorR3* targets); const MatrixRmn& ActiveJacobian() const { return *Jactive; } @@ -69,7 +70,7 @@ public: void CalcDeltaThetasDLS(); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); - void CalcThetasDotDLS(float dt); + void UpdateThetas(); void UpdateThetaDot(); @@ -90,8 +91,8 @@ public: int GetNumCols() {return nCol;} public: - Tree* tree; // tree associated with this Jacobian matrix - int nEffector; // Number of end effectors + Tree* m_tree; // tree associated with this Jacobian matrix + int m_nEffector; // Number of end effectors int nJoint; // Number of joints int nRow; // Total number of rows the real J (= 3*number of end effectors for now) int nCol; // Total number of columns in the real J (= number of joints for now) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f73c16a36..9e82744fa 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1018,6 +1018,24 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { return 0; } +// vector - double[3] which will be set by values from obVec +static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) { + int i, len; + PyObject* seq; + + seq = PySequence_Fast(obVec, "expected a sequence"); + len = PySequence_Size(obVec); + if (len == 4) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + return 0; +} + static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { int size = PySequence_Size(args); int objectUniqueIdA = -1; @@ -1658,22 +1676,25 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self, if (size == 2) { int bodyIndex; + int endEffectorLinkIndex; + PyObject* targetPosObj; - - if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetPosObj)) + PyObject* targetOrnObj; + + if (PyArg_ParseTuple(args, "iiOO", &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj)) { double pos[3]; double ori[4]={0,1.0,0,0}; - double dt=0.0001; - if (pybullet_internalSetVectord(targetPosObj,pos)) + if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4(targetOrnObj,ori)) { b3SharedMemoryStatusHandle statusHandle; int numPos=0; int resultBodyIndex; int result; - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex, - pos,ori,dt); + + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex); + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); result = b3GetStatusInverseKinematicsJointPositions(statusHandle,