Why is the link position not updating in this Reduced Articulation example

Question

I am trying to update the state of a ReducedArticulation (with PhysX5) without updating the simulation (I would rather not have the simulation try to run)

std::pair<PxArticulationReducedCoordinate*, std::vector<PxArticulationJointReducedCoordinate*> > createArm(PxPhysics* physics)
{
  // create the material
  PxMaterial* material = physics->createMaterial(0.5f, 0.8f, 0.3f);
  std::vector<PxArticulationJointReducedCoordinate*> joints;

  PxArticulationReducedCoordinate* articulation = physics->createArticulationReducedCoordinate();
  // create base
  // make everything out of capsules around the arm
  PxArticulationLink* base = articulation->createLink(NULL, PxTransform(PxVec3(0.f, 0.0f, 0.0f)));
  PxRigidActorExt::createExclusiveShape(*base, PxBoxGeometry(.5f, .1f, .5f), *material);
  PxRigidBodyExt::updateMassAndInertia(*base, 1.f);

  // create shoulder
  PxArticulationLink* shoulder = articulation->createLink(base, PxTransform(PxVec3(-0.1, -0.2f, 0.1f)));
  PxRigidActorExt::createExclusiveShape(*shoulder, PxBoxGeometry(0.1f, 1.f, 0.1f), *material);
  PxRigidBodyExt::updateMassAndInertia(*shoulder , 100.f);

  // create elbow
  PxArticulationLink* elbow = articulation->createLink(shoulder, PxTransform(PxVec3(0.f, 0.0f, 1.f)));
  PxRigidActorExt::createExclusiveShape(*elbow, PxBoxGeometry(0.1f, 0.1f, 1.0f), *material);
  PxRigidBodyExt::updateMassAndInertia(*elbow, 100.f);

  PxQuat rotationQuat(PxIdentity);
  const PxVec3 rightAnchorLocation = PxVec3(0.f, 1.f, 0.9f);

  PxArticulationJointReducedCoordinate* joint = static_cast<PxArticulationJointReducedCoordinate*>(shoulder->getInboundJoint());
  joint->setParentPose(PxTransform(shoulder->getGlobalPose().transformInv(rightAnchorLocation), rotationQuat));
  joint->setChildPose(PxTransform(PxVec3(0.f, 1.f, 0.f), rotationQuat));
  joint->setJointType(PxArticulationJointType::eREVOLUTE);
  joint->setMotion(PxArticulationAxis::eTWIST, PxArticulationMotion::eLIMITED);
  joint->setLimit(PxArticulationAxis::eTWIST, -PxPi, PxPi);
  joints.push_back(joint);

  joint = static_cast<PxArticulationJointReducedCoordinate*>(elbow->getInboundJoint());
  joint->setParentPose(PxTransform(elbow->getGlobalPose().transformInv(rightAnchorLocation), rotationQuat));
  joint->setChildPose(PxTransform(PxVec3(0.f, 0.f, 1.f), rotationQuat));
  joint->setJointType(PxArticulationJointType::eREVOLUTE);
  joint->setMotion(PxArticulationAxis::eTWIST, PxArticulationMotion::eFREE);
  joints.push_back(joint);


  return std::make_pair(articulation, joints);
}

int main()
{
  PxDefaultAllocator allocator;
  PxDefaultErrorCallback errorCallback;
  PxFoundation* foundation = PxCreateFoundation(PX_PHYSICS_VERSION, allocator, errorCallback);
  PxPhysics* physics = PxCreatePhysics(PX_PHYSICS_VERSION, *foundation, PxTolerancesScale(), true, nullptr);
  PxDefaultCpuDispatcher* gDispatcher;

  PxSceneDesc sceneDesc(physics->getTolerancesScale());
  //sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);

  PxU32 numCores = 2;
  gDispatcher = PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1);
  sceneDesc.cpuDispatcher = gDispatcher;
  sceneDesc.filterShader  = PxDefaultSimulationFilterShader;

  sceneDesc.solverType = PxSolverType::eTGS;
  PxScene* scene;
  scene = physics->createScene(sceneDesc);
  PxGetPhysics().getScenes(&scene,1);
  auto arm_and_joints = createArm(physics);
  scene->addArticulation(*arm_and_joints.first);
  // create a cache to grab FK results
  // https://nvidia-omniverse.github.io/PhysX/physx/5.1.0/_build/physx/latest/class_px_articulation_cache.html#_CPPv419PxArticulationCache
  PxArticulationCache* cache = arm_and_joints.first->createCache();
  // move joints
  arm_and_joints.second[1]->setJointPosition(PxArticulationAxis::eTWIST, 1.5);
  // grab cache
  arm_and_joints.first->copyInternalStateToCache(*cache, PxArticulationCacheFlag::ePOSITION);

  // get the second link and check position
  PxArticulationLink* link2;
  arm_and_joints.first->getLinks(&link2, 1, 1);
  auto pose = link2->getGlobalPose();
  std::cout << "X" << pose.p.x << "Y" << pose.p.y << "Z" << pose.p.z << std::endl;
  std::cout << "X" << pose.q.x << "Y" << pose.q.y << "Z" << pose.q.z << std::endl;
  arm_and_joints.second[1]->setJointPosition(PxArticulationAxis::eTWIST, -.5);
  arm_and_joints.second[0]->setJointPosition(PxArticulationAxis::eTWIST, -.5);
  arm_and_joints.first->getLinks(&link2, 1, 1);
  pose = link2->getGlobalPose();
  std::cout << "X" << pose.p.x << "Y" << pose.p.y << "Z" << pose.p.z << std::endl;
  std::cout << "X" << pose.q.x << "Y" << pose.q.y << "Z" << pose.q.z << std::endl;

}

The output shows that the link of the position hasn't changed

X0.1Y0.2Z0.8
X0Y0Z0
X0.1Y0.2Z0.8
X0Y0Z0

From the docs here they say:

https://nvidia-omniverse.github.io/PhysX/physx/5.1.0/docs/Articulations.html#pxarticulationcache

Note that this will cause the link poses to be updated based on the newly set joint positions, and it is not legal to copy to or apply a cache while the simulation is running.

And I thought setJointPosition would update the cache

Why is the link position not updating in this Reduced Articulation example

0 Answers0