Skip to content

Commit

Permalink
physx quatBall fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@MarcToussaint
MarcToussaint committed Nov 30, 2024
1 parent c01cd48 commit 6f1a46e
Show file tree
Hide file tree
Showing 5 changed files with 85 additions and 43 deletions.
26 changes: 21 additions & 5 deletions src/Kin/frame.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -283,11 +283,10 @@ void rai::Frame::prefixSubtree(const char* prefix) {
}

void rai::Frame::computeCompoundInertia(bool clearChildInertias) {
CHECK(!inertia, "this frame already has inertia");
FrameL all = {};
getRigidSubFrames(all, false);
Inertia* I = new Inertia(*this);
I->setZero();
Inertia* I = inertia;
if(!I){ I = new Inertia(*this); I->setZero(); }
for(rai::Frame* f:all) if(f->inertia) {
I->add(*f->inertia, f->ensure_X() / ensure_X());
if(clearChildInertias) delete f->inertia;
Expand Down Expand Up @@ -1561,7 +1560,7 @@ void rai::Joint::setType(rai::JointType _type) {
dim = getDimFromType();
frame->C.reset_q();
q0 = calcDofsFromConfig();
isPartBreak = !((type>=JT_hingeX && type<=JT_hingeZ) || (type>=JT_transX && type<=JT_transZ));
isPartBreak = !((type>=JT_hingeX && type<=JT_hingeZ) || (type>=JT_transX && type<=JT_transZ) || type==JT_quatBall);
}
}

Expand Down Expand Up @@ -1640,7 +1639,7 @@ void rai::Joint::read(const Graph& ats) {
else type=JT_rigid;

dim = getDimFromType();
isPartBreak = !((type>=JT_hingeX && type<=JT_hingeZ) || (type>=JT_transX && type<=JT_transZ));
isPartBreak = !((type>=JT_hingeX && type<=JT_hingeZ) || (type>=JT_transX && type<=JT_transZ) || type==JT_quatBall);

if(ats.get(d, "q")) {
if(!dim) { //HACK convention
Expand Down Expand Up @@ -2072,6 +2071,23 @@ void rai::Inertia::defaultInertiaByShape() {
}
}

rai::Transformation rai::Inertia::getDiagTransform(arr& diag){
rai::Transformation t=0;
if(!com.isZero){
t.pos = com;
}
if(!matrix.isDiagonal()) {
arr I = matrix.getArr();
arr U, d, V;
svd(U, d, V, I, false);
t.rot.setMatrix(V);
if(!!diag) diag=d;
}else{
if(!!diag) diag = arr{matrix.m00, matrix.m11, matrix.m22};
}
return t;
}

void rai::Inertia::write(std::ostream& os) const {
os <<", mass: " <<mass;
if(!com.isZero) {
Expand Down
4 changes: 3 additions & 1 deletion src/Kin/frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -299,7 +299,7 @@ stdOutPipe(Joint)
/// a Frame with Inertia has mass and, in physical simulation, has forces associated with it
struct Inertia : NonCopyable {
Frame& frame;
double mass=-1.;
double mass=0.;
Matrix matrix=0;
Enum<BodyType> type;
Vector com=0; ///< its center of mass
Expand All @@ -311,6 +311,8 @@ struct Inertia : NonCopyable {
void add(const Inertia& I, const rai::Transformation& rel);
void defaultInertiaByShape();

rai::Transformation getDiagTransform(arr& diag);

void write(std::ostream& os) const;
void write(Graph& g);
void read(const Graph& G);
Expand Down
87 changes: 55 additions & 32 deletions src/Kin/kin_physx.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -453,6 +453,7 @@ void PhysXInterface_self::addMultiBody(rai::Frame* base) {

//decide on options
bool multibody_floating = base->ats->findNode("multibody_floating");
bool multibody_gravity = base->ats->findNode("multibody_gravity");

//-- collect all links for that root
FrameL F = {base};
Expand Down Expand Up @@ -506,7 +507,7 @@ void PhysXInterface_self::addMultiBody(rai::Frame* base) {

addShapesAndInertia(actor, shapes, type, f);

if(opt.multiBodyDisableGravity && !multibody_floating) {
if(opt.multiBodyDisableGravity && !multibody_gravity) {
actor->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, true);
}

Expand Down Expand Up @@ -564,16 +565,26 @@ void PhysXInterface_self::addMultiBody(rai::Frame* base) {
axis = PxArticulationAxis::eZ;
break;
}
// case rai::JT_quatBall:{
// type = PxArticulationJointType::eSPHERICAL;
// axis = PxArticulationAxis::eZ;
// break;
// }
case rai::JT_quatBall:{
type = PxArticulationJointType::eSPHERICAL;
axis = PxArticulationAxis::eCOUNT;
break;
}
default: NIY;
}
joint->setJointType(type);
joint->setMotion(axis, PxArticulationMotion::eFREE); //eLIMITED
joint->setJointPosition(axis, f->joint->scale*f->joint->get_q());
if(axis!=PxArticulationAxis::eCOUNT){
joint->setMotion(axis, PxArticulationMotion::eFREE); //eLIMITED
joint->setJointPosition(axis, f->joint->scale*f->joint->get_q());
}else{
joint->setMotion(PxArticulationAxis::eTWIST, PxArticulationMotion::eFREE); //eLIMITED
joint->setMotion(PxArticulationAxis::eSWING1, PxArticulationMotion::eFREE); //eLIMITED
joint->setMotion(PxArticulationAxis::eSWING2, PxArticulationMotion::eFREE); //eLIMITED
auto vec = f->get_Q().rot.getVec();
joint->setJointPosition(PxArticulationAxis::eTWIST, vec.x);
joint->setJointPosition(PxArticulationAxis::eSWING1, vec.y);
joint->setJointPosition(PxArticulationAxis::eSWING2, vec.z);
}
jointAxis(f->ID) = axis;

// if(f->joint->limits.N){
Expand All @@ -592,7 +603,7 @@ void PhysXInterface_self::addMultiBody(rai::Frame* base) {
// world->addMultiBodyConstraint(gearCons);
}

if(true) {
if(axis!=PxArticulationAxis::eCOUNT) { //only 1D joints have drives!
PxArticulationDrive posDrive;
if(f->joint->active) {
posDrive.stiffness = opt.motorKp; // the spring constant driving the joint to a target position
Expand All @@ -610,6 +621,8 @@ void PhysXInterface_self::addMultiBody(rai::Frame* base) {
}
}

articulation->updateKinematic(PxArticulationKinematicFlag::ePOSITION);

gScene->addArticulation(*articulation);

if(opt.verbose>0) {
Expand All @@ -636,9 +649,9 @@ void PhysXInterface_self::prepareLinkShapes(ShapeL& shapes, rai::BodyType& type,
bool shapesHaveInertia=false;
for(rai::Shape* s:shapes) if(s->frame.inertia) { shapesHaveInertia=true; break; }
if(shapesHaveInertia && !f->inertia) {
LOG(-1) <<"computing compound inertia for object frame '" <<f->name <<"' -- this should have been done earlier?";
LOG(-1) <<"computing compound inertia for object frame '" <<f->name;
f->computeCompoundInertia();
f->transformToDiagInertia();
// f->transformToDiagInertia(); //the inertial needs to be at that link... not just a child...
}
if(f->inertia && !f->inertia->matrix.isDiagonal()) {
LOG(-1) <<"DON'T DO THAT! PhysX can only properly handle (compound) inertias if transformed to diagonal tensor\n frame:" <<*f;
Expand Down Expand Up @@ -776,8 +789,17 @@ void PhysXInterface_self::addShapesAndInertia(PxRigidBody* actor, ShapeL& shapes
if(f->inertia && f->inertia->mass>0.) {
//PxRigidBodyExt::updateMassAndInertia(*actor, f->inertia->mass);
actor->setMass(f->inertia->mass);
actor->setMassSpaceInertiaTensor({float(f->inertia->matrix.m00), float(f->inertia->matrix.m11), float(f->inertia->matrix.m22)});
//cout <<*f->inertia <<" m:" <<actor->getMass() <<" I:" <<conv_PxVec3_arr(actor->getMassSpaceInertiaTensor()) <<endl;
if(!f->inertia->com && f->inertia->matrix.isDiagonal()){
actor->setMassSpaceInertiaTensor({float(f->inertia->matrix.m00), float(f->inertia->matrix.m11), float(f->inertia->matrix.m22)});
}else{
arr I;
rai::Transformation t = f->inertia->getDiagTransform(I);
if(!t.pos.isZero || !t.rot.isZero){
actor->setCMassLocalPose(conv_Transformation2PxTrans(t));
}
actor->setMassSpaceInertiaTensor({float(I(0)), float(I(1)), float(I(2))});
}
// //cout <<*f->inertia <<" m:" <<actor->getMass() <<" I:" <<conv_PxVec3_arr(actor->getMassSpaceInertiaTensor()) <<endl;
} else {
PxRigidBodyExt::updateMassAndInertia(*actor, 1000.f);
if(!f->inertia) new rai::Inertia(*f);
Expand Down Expand Up @@ -937,19 +959,19 @@ void PhysXInterface::pushMotorStates(const rai::Configuration& C, bool setInstan
if(!joint) continue;

auto axis = self->jointAxis(f->ID);
CHECK_LE(axis, self->jointAxis(0)-1, "");

if(setInstantly) joint->setJointPosition(axis, f->joint->scale*f->joint->get_q());
joint->setDriveTarget(axis, f->joint->scale*f->joint->get_q());

if(!!qDot && qDot.N) { //also setting vel reference!
if(setInstantly) joint->setJointVelocity(axis, f->joint->scale*qDot(f->joint->qIndex));
joint->setDriveVelocity(axis, f->joint->scale*qDot(f->joint->qIndex));
} else {
if(setInstantly) joint->setJointVelocity(axis, 0.);
joint->setDriveVelocity(axis, 0.);
if(axis!=PxArticulationAxis::eCOUNT){ //only joints with drive
if(setInstantly) joint->setJointPosition(axis, f->joint->scale*f->joint->get_q());
joint->setDriveTarget(axis, f->joint->scale*f->joint->get_q());

if(!!qDot && qDot.N) { //also setting vel reference!
if(setInstantly) joint->setJointVelocity(axis, f->joint->scale*qDot(f->joint->qIndex));
joint->setDriveVelocity(axis, f->joint->scale*qDot(f->joint->qIndex));
} else {
if(setInstantly) joint->setJointVelocity(axis, 0.);
joint->setDriveVelocity(axis, 0.);
}
}
}
}
} else if(self->opt.jointedBodies) {
NIY;
}
Expand All @@ -969,14 +991,15 @@ void PhysXInterface::pullMotorStates(rai::Configuration& C, arr& qDot) {
if(!joint) continue;

auto axis = self->jointAxis(f->ID);
CHECK_LE(axis, self->jointAxis(0)-1, "");
if(f->joint->active){
q(f->joint->qIndex) = joint->getJointPosition(axis) / f->joint->scale;
if(!!qDot) qDot(f->joint->qIndex) = joint->getJointVelocity(axis) / f->joint->scale;
}else{
qInactive(f->joint->qIndex) = joint->getJointPosition(axis) / f->joint->scale;
if(axis!=PxArticulationAxis::eCOUNT){ //only joints with drive
if(f->joint->active){
q(f->joint->qIndex) = joint->getJointPosition(axis) / f->joint->scale;
if(!!qDot) qDot(f->joint->qIndex) = joint->getJointVelocity(axis) / f->joint->scale;
}else{
qInactive(f->joint->qIndex) = joint->getJointPosition(axis) / f->joint->scale;
}
}
}
}
} else if(self->opt.jointedBodies) {
NIY;
}
Expand Down
2 changes: 1 addition & 1 deletion src/Kin/simulation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,7 @@ void Simulation::step(const arr& u_control, double tau, ControlMode u_mode) {
}
self->physx->step(tau);
self->physx->pullDynamicStates(C, self->frameVelocities);
self->physx->pullMotorStates(C, self->qDot); //why not also pull the motor states?
self->physx->pullMotorStates(C, self->qDot);
} else if(engine==_bullet) {
self->bullet->pushKinematicStates(C);
if(self->bullet->opt().multiBody) {
Expand Down
9 changes: 5 additions & 4 deletions test/Kin/simulation/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -482,9 +482,9 @@ void testMotors(){

//===========================================================================

void testG1(){
void testPassive(const char* filename){
rai::Configuration C;
C.addFile("../../../../playground/24-humanoid/scene.g");
C.addFile(filename);
// C.optimizeTree(true);

rai::Simulation S(C, S._physx, 3);
Expand All @@ -499,7 +499,7 @@ void testG1(){

S.step({}, tau, S._none);

// C.view(false, STRING("time:" <<t));
// C.view(true, STRING("time:" <<t));
}
}

Expand Down Expand Up @@ -536,9 +536,10 @@ void testSplineMode(){
int MAIN(int argc,char **argv){
rai::initCmdLine(argc, argv);

testG1(); return 0;

testMotors();
testPassive("../../../../playground/24-humanoid/scene.g");
testPassive("../../../../rai-robotModels/scenarios/pendulum.g");
testRndScene();
testConstructor();
testPcl();
Expand Down

0 comments on commit 6f1a46e

Please sign in to comment.