Ian Chen avatar Ian Chen committed e35742e

Test manually accumulate joint torque

Comments (0)

Files changed (4)

gazebo/physics/bullet/BulletHingeJoint.cc

   // If both links exist, then create a joint between the two links.
   if (bulletChildLink && bulletParentLink)
   {
+    gzerr << "pivots child parent " << pivotChild << ", " << pivotParent << std::endl;
     this->bulletHinge = new btHingeConstraint(
         *(bulletChildLink->GetBulletLink()),
         *(bulletParentLink->GetBulletLink()),

gazebo/physics/bullet/BulletJoint.cc

     gzerr << "force " << wrench.body1Force << " , " << wrench.body2Force << std::endl;
     gzerr << "torque " << wrench.body1Torque << " , " << wrench.body2Torque << std::endl;
 
+/*    gzerr << " existing torque " <<
+      BulletTypes::ConvertVector3(this->constraint->getRigidBodyA().getTotalTorque())
+      << " , " <<
+      BulletTypes::ConvertVector3(this->constraint->getRigidBodyB().getTotalTorque())
+      << std::endl;*/
+
+    gzerr << "anchor " << this->anchorPos << ", " << this->anchorPose << " "<< std::endl;
+
 /*    gzerr << " bt torque " << fb->m_appliedTorqueBodyB.x() << " " << fb->m_appliedTorqueBodyB.y() << " " << fb->m_appliedTorqueBodyB.z() << std::endl;
 
     gzerr << "anchor " << this->anchorPos << ", " << this->anchorPose << " "<< std::endl;
       //       << "] fxp[" << wrench.body2Force.Cross(childMomentArm)
       //       << "]\n";
 
+      btRigidBody bodyA = this->constraint->getRigidBodyA();
+      int numConstraints = bodyA.getNumConstraintRefs();
+      btTypedConstraint *otherConstraint;
+        for ( int i = 0; i < numConstraints; ++i)
+        {
+          otherConstraint = bodyA.getConstraintRef(i);
+          if ( otherConstraint != this->constraint)
+          {
+            if (otherConstraint->getRigidBodyA().getUserPointer()
+                == bodyA.getUserPointer())
+            {
+              wrench.body1Torque += BulletTypes::ConvertVector3(
+                  otherConstraint->getJointFeedback()->
+                  m_appliedTorqueBodyB);
+            }
+            else
+            {
+              wrench.body1Torque += BulletTypes::ConvertVector3(
+                  otherConstraint->getJointFeedback()->
+                  m_appliedTorqueBodyA);
+            }
+          }
+        }
+
       wrench.body2Torque += wrench.body2Force.Cross(childMomentArm);
 
+
       // rotate resulting body1Force in world frame into link frame
       wrench.body2Force = childPose.rot.RotateVectorReverse(
         -wrench.body2Force);
       // rotate resulting body1Torque in world frame into link frame
       wrench.body2Torque = childPose.rot.RotateVectorReverse(
         -wrench.body2Torque);
+
+      gzerr << "b2t child modment arm " << childMomentArm << ", " << wrench.body2Force << ", " << wrench.body2Torque << std::endl;
     }
 
     // convert torque from about parent CG to joint anchor location
 
 //      gzerr << " parent link b4 body1torque " << wrench.body1Torque << std::endl;
 
+      btRigidBody bodyB = this->constraint->getRigidBodyB();
+      int numConstraints = bodyB.getNumConstraintRefs();
+      btTypedConstraint *otherConstraint;
+        for ( int i = 0; i < numConstraints; ++i)
+        {
+          otherConstraint = bodyB.getConstraintRef(i);
+          if ( otherConstraint != this->constraint)
+          {
+            if (otherConstraint->getRigidBodyA().getUserPointer()
+                == bodyB.getUserPointer())
+            {
+              wrench.body1Torque += BulletTypes::ConvertVector3(
+                  otherConstraint->getJointFeedback()->
+                  m_appliedTorqueBodyB);
+            }
+            else
+            {
+              wrench.body1Torque += BulletTypes::ConvertVector3(
+                  otherConstraint->getJointFeedback()->
+                  m_appliedTorqueBodyA);
+            }
+          }
+        }
+
       wrench.body1Torque += wrench.body1Force.Cross(parentMomentArm);
 
+      if ( this->constraint->getRigidBodyB().getNumConstraintRefs() > 1)
+      {
+        gzerr << " another joint constraint b1a " <<
+        BulletTypes::ConvertVector3(
+        this->constraint->getRigidBodyB().getConstraintRef(1)->getJointFeedback()->m_appliedTorqueBodyA) << std::endl;
+        gzerr << " another joint constraint b1b " <<
+        BulletTypes::ConvertVector3(
+        this->constraint->getRigidBodyB().getConstraintRef(1)->getJointFeedback()->m_appliedTorqueBodyB) << std::endl;
+        gzerr << " another joint constraint b0a " <<
+        BulletTypes::ConvertVector3(
+        this->constraint->getRigidBodyB().getConstraintRef(0)->getJointFeedback()->m_appliedTorqueBodyA) << std::endl;
+        gzerr << " another joint constraint b0b " <<
+        BulletTypes::ConvertVector3(
+        this->constraint->getRigidBodyB().getConstraintRef(0)->getJointFeedback()->m_appliedTorqueBodyB) << std::endl;
+      }
+
+
+      gzerr << "b2t parent modment arm " << parentMomentArm << ", " << wrench.body1Force << ", " << wrench.body1Torque << std::endl;
+
 //      gzerr << " parent link body1torque " << wrench.body1Torque << std::endl;
 //      gzerr << " parentMomentArm " << parentMomentArm << std::endl;
 //      gzerr << " cgPos " << cgPos << std::endl;

gazebo/physics/bullet/BulletJoint_TEST.cc

   ASSERT_TRUE(physics != NULL);
   EXPECT_EQ(physics->GetType(), "bullet");
 
-  //physics->SetGravity(math::Vector3(0, 0, -50));
+  physics->SetGravity(math::Vector3(0, 0, -50));
 
   // simulate 1 step
   world->StepWorld(1);
 
   math::Vector3 gravity(-30, 10, -50);
 
-  physics->SetGravity(math::Vector3(gravity.x, 0, 0));
+//  physics->SetGravity(math::Vector3(gravity.x, 0, 0));
+  physics->SetGravity(gravity);
   // tune joint stop properties
   joint_01->SetAttribute("stop_erp", 0, 0.02);
   joint_12->SetAttribute("stop_erp", 0, 0.02);
   // wait for dynamics to stabilize
   world->StepWorld(2000);
 
+
+
 //  gzerr << "child after "
 //    << joint_12->GetChild()->GetWorldCoGPose().pos  << std::endl;
 

gazebo/physics/ode/ODEJoint.cc

     wrench.body1Force.Set(fb->f2[0], fb->f2[1], fb->f2[2]);
     wrench.body1Torque.Set(fb->t2[0], fb->t2[1], fb->t2[2]);
 
-/*    gzerr << "force " << wrench.body1Force << " , " << wrench.body2Force << std::endl;
+    gzerr << "anchor " << this->anchorPos << ", " << this->anchorPose << " "<< std::endl;
+
+    gzerr << "force " << wrench.body1Force << " , " << wrench.body2Force << std::endl;
     gzerr << "torque " << wrench.body1Torque << " , " << wrench.body2Torque << std::endl;
 
-    gzerr << "anchor " << this->anchorPos << ", " << this->anchorPose << " "<< std::endl;
+/*    gzerr << "anchor " << this->anchorPos << ", " << this->anchorPose << " "<< std::endl;
     gzerr << "global axis " << this->GetGlobalAxis(0) << std::endl;
     gzerr << "angle " << this->GetAngle(0) << std::endl;*/
 
 
       wrench.body2Torque += wrench.body2Force.Cross(childMomentArm);
 
+      gzerr << "b2t child modment arm " << childMomentArm << ", " << wrench.body2Force << ", " << wrench.body2Torque << std::endl;
+
       // rotate resulting body1Force in world frame into link frame
       wrench.body2Force = childPose.rot.RotateVectorReverse(
         -wrench.body2Force);
 
       wrench.body1Torque += wrench.body1Force.Cross(parentMomentArm);
 
-      gzerr << " parent link body1torque " << wrench.body1Torque << std::endl;
+      gzerr << "b2t parent modment arm " << parentMomentArm << ", " << wrench.body1Force << ", " << wrench.body1Torque << std::endl;
+
+      /*gzerr << " parent link body1torque " << wrench.body1Torque << std::endl;
       gzerr << " parentMomentArm " << parentMomentArm << std::endl;
-      gzerr << " cgPos " << cgPos << std::endl;
+      gzerr << " cgPos " << cgPos << std::endl;*/
 
       // rotate resulting body1Force in world frame into link frame
       wrench.body1Force = parentPose.rot.RotateVectorReverse(
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.