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

Merge pull request #800 from bulletphysics/iktest

Browse Source 
Iktest
This commit is contained in:
erwincoumans
2016-09-23 07:40:09 -07:00
committed by GitHub
parent 3df9d6c4ee b6bb937dc0
commit 666c8f47b7
19 changed files with 635 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files

303
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -29,6 +29,8 @@
btVector3 gLastPickPos(0, 0, 0);
bool gEnableRealTimeSimVR=false;
int gCreateObjectSimVR = -1;
btScalar simTimeScalingFactor = 1;
struct UrdfLinkNameMapUtil
{
@@ -387,6 +389,8 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
int m_huskyId;
int m_KukaId;
int m_sphereId;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -438,7 +442,9 @@ struct PhysicsServerCommandProcessorInternalData
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(false),
m_huskyId(0),
m_huskyId(-1),
m_KukaId(-1),
m_sphereId(-1),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
@@ -558,6 +564,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
}
@@ -746,7 +753,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
if (supportsJointMotor(mb,mbLinkIndex))
{
float maxMotorImpulse = 10000.f;
float maxMotorImpulse = 1.f;
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
@@ -1880,13 +1887,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
applyJointDamping(i);
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
if (m_data->m_numSimulationSubSteps > 0)
{
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
}
else
{
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, 0);
m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
}
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2546,7 +2555,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);
@@ -2560,41 +2569,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 && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
int numJoints1 = bodyHandle->m_multiBody->getNumLinks();
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*7);
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> 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<double> 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);
@@ -2608,24 +2616,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<double> 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;
@@ -2639,15 +2648,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;i<numJoints;i++)
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numJoints;
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
}
@@ -2848,8 +2858,12 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
btVector3 gVRGripperPos(0,0,0.2);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);;
btQuaternion gVRController2Orn(0,0,0,1);
btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false;
@@ -2864,14 +2878,29 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int bodyId = 0;
if (gCreateObjectSimVR >= 0)
{
gCreateObjectSimVR = -1;
btMatrix3x3 mat(gVRGripperOrn);
btScalar spawnDistance = 0.1;
btVector3 spawnDir = mat.getColumn(0);
btVector3 shiftPos = spawnDir*spawnDistance;
btVector3 spawnPos = gVRGripperPos + shiftPos;
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_sphereId = bodyId;
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setBaseVel(spawnDir * 3);
}
}
if (!m_data->m_hasGround)
{
m_data->m_hasGround = true;
int bodyId = 0;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -2913,13 +2942,14 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("cube.urdf", btVector3(3, -2, 0.5+i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -2950,7 +2980,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
loadUrdf("teddy_vhacd.urdf", btVector3(1, 1, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
@@ -2989,6 +3020,196 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
}
{
InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btVector3 spherePos(0,0,0);
InternalBodyHandle* sphereBodyHandle = m_data->getHandle(m_data->m_KukaId);
if (sphereBodyHandle && sphereBodyHandle->m_multiBody)
{
spherePos = sphereBodyHandle->m_multiBody->getBasePos();
}
btMultiBody* mb = bodyHandle->m_multiBody;
btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2();
btScalar distanceThreshold = 2;
bool closeToKuka=(sqLen<(distanceThreshold*distanceThreshold));
int numDofs = bodyHandle->m_multiBody->getNumDofs();
btAlignedObjectArray<double> q_new;
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
}
q_new.resize(numDofs);
static btScalar t=0.f;
t+=0.01;
double dampIk = 0.99;
for (int i=0;i<numDofs;i++)
{
btScalar desiredPosition = btSin(t*0.1)*SIMD_HALF_PI;
q_new[i] = dampIk*q_current[i]+(1-dampIk)*desiredPosition;
}
if (closeToKuka)
{
dampIk = 1;
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;
if (ikHelperPtrPtr)
{
ikHelperPtr = *ikHelperPtrPtr;
}
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
}
int endEffectorLinkIndex = 6;
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
int numJoints1 = bodyHandle->m_multiBody->getNumLinks();
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
jacSize = jacobian_linear.size();
// Set jacobian value
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
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);
qdot[i + baseDofs] = 0;
nu[i+baseDofs] = 0;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
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 < numDofs; ++j)
{
jacobian_linear[i*numDofs+j] = jac_t(i,j);
jacobian_angular[i*numDofs+j] = jac_r(i,j);
}
}
}
}
//int ikMethod= IK2_VEL_DLS;//IK2_VEL_DLS_WITH_ORIENTATION;//IK2_VEL_DLS;
int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION;//IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3DoubleData targetWorldPosition;
btQuaternionDoubleData targetWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
gVRController2Pos.serializeDouble(targetWorldPosition);
gVRController2Orn.serializeDouble(targetWorldOrientation);
static btScalar time=0.f;
time+=0.01;
btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time));
targetPos +=mb->getBasePos();
btQuaternion fwdOri(btVector3(1,0,0),-SIMD_HALF_PI);
(0, 1.0, 0, 0);
double downOrn[4] = {0,1,0,0};
//double downOrn[4] = {0,1,0,0};
fwdOri.serializeDouble(targetWorldOrientation);
ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk);
}
}
//directly set the position of the links, only for debugging IK, don't use this method!
//if (0)
//{
// for (int i=0;i<mb->getNumLinks();i++)
// {
// mb->setJointPosMultiDof(i,&q_new[i]);
// }
//} else
{
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for (int link=0;link<mb->getNumLinks();link++)
{
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
btScalar desiredVelocity = 0.f;
btScalar desiredPosition = q_new[link];
motor->setVelocityTarget(desiredVelocity,1);
motor->setPositionTarget(desiredPosition,0.6);
btScalar maxImp = 1.f;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
}
velIndex += mb->getLink(link).m_dofCount;
posIndex += mb->getLink(link).m_posVarCount;
}
}
}
}
}
int maxSteps = m_data->m_numSimulationSubSteps+3;
if (m_data->m_numSimulationSubSteps)
{
@@ -2999,7 +3220,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
gSubStep = m_data->m_physicsDeltaTime;
}
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep);
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec*simTimeScalingFactor,maxSteps, gSubStep);
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps)