All Downloads are FREE. Search and download functionalities are using the official Maven repository.

us.ihmc.scs2.simulation.bullet.physicsEngine.BulletMultiBodyRobotFactory Maven / Gradle / Ivy

The newest version!
package us.ihmc.scs2.simulation.bullet.physicsEngine;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import org.bytedeco.bullet.BulletCollision.btCollisionShape;
import org.bytedeco.bullet.BulletCollision.btCompoundShape;
import org.bytedeco.bullet.BulletDynamics.btMultiBodyJointLimitConstraint;
import org.bytedeco.bullet.BulletDynamics.btMultiBodyLinkCollider;
import org.bytedeco.bullet.BulletDynamics.btMultibodyLink;
import org.bytedeco.bullet.LinearMath.btQuaternion;
import org.bytedeco.bullet.LinearMath.btTransform;
import org.bytedeco.bullet.LinearMath.btVector3;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.scs2.definition.collision.CollisionShapeDefinition;
import us.ihmc.scs2.definition.robot.JointDefinition;
import us.ihmc.scs2.definition.robot.PrismaticJointDefinition;
import us.ihmc.scs2.definition.robot.RevoluteJointDefinition;
import us.ihmc.scs2.definition.robot.RigidBodyDefinition;
import us.ihmc.scs2.simulation.bullet.physicsEngine.parameters.YoBulletMultiBodyJointParameters;
import us.ihmc.scs2.simulation.bullet.physicsEngine.parameters.YoBulletMultiBodyParameters;
import us.ihmc.scs2.simulation.robot.Robot;
import us.ihmc.scs2.simulation.robot.multiBodySystem.SimFloatingRootJoint;
import us.ihmc.scs2.simulation.robot.multiBodySystem.SimPrismaticJoint;
import us.ihmc.scs2.simulation.robot.multiBodySystem.SimRevoluteJoint;

public class BulletMultiBodyRobotFactory
{
   private static HashMap jointNameToBulletJointIndexMap;
   private static int linkCountingIndex;
   private static int numberOfLinks;

   public static BulletMultiBodyRobot newInstance(Robot robot,
                                                  YoBulletMultiBodyParameters bulletMultiBodyParameters,
                                                  YoBulletMultiBodyJointParameters bulletMultiBodyJointParameters)
   {
      if (robot.getRootBody().getChildrenJoints().size() == 0)
      {
         throw new UnsupportedOperationException("Robot must have at least one joint: " + robot.getClass().getSimpleName());
      }

      JointBasics rootJoint = robot.getRootBody().getChildrenJoints().get(0);
      boolean hasBaseCollider = rootJoint instanceof SimFloatingRootJoint;

      jointNameToBulletJointIndexMap = new HashMap();
      linkCountingIndex = hasBaseCollider ? 0 : 1;
      numberOfLinks = 0;
      for (JointBasics joint : robot.getRootBody().getChildrenJoints())
      {
         numberOfLinks += countJointsAndCreateIndexMap(joint) - (hasBaseCollider ? 1 : 0);
      }

      RigidBodyDefinition rigidBodyDefinition = robot.getRobotDefinition().getRootBodyDefinition().getChildrenJoints().get(0).getSuccessor();
      double rootBodyMass = rigidBodyDefinition.getMass();

      //      TODO: fix moment of inertia using SVD      
      //      RotationMatrix svd_rotation = new RotationMatrix();
      //      Vector3 rootBodyIntertia = decomposeMomentOfInertia(svd_rotation, rigidBodyDefinition);
      btVector3 rootBodyIntertia = new btVector3(rigidBodyDefinition.getMomentOfInertia().getM00(),
                                                 rigidBodyDefinition.getMomentOfInertia().getM11(),
                                                 rigidBodyDefinition.getMomentOfInertia().getM22());
      boolean fixedBase = hasBaseCollider ? false : true;
      BulletMultiBodyRobot bulletMultiBodyRobot = new BulletMultiBodyRobot(numberOfLinks,
                                                                           rootBodyMass,
                                                                           rootBodyIntertia,
                                                                           fixedBase,
                                                                           bulletMultiBodyParameters.getCanSleep(),
                                                                           jointNameToBulletJointIndexMap);

      for (JointBasics joint : robot.getRootBody().getChildrenJoints())
      {
         int linkIndex = bulletMultiBodyRobot.getJointNameToBulletJointIndexMap().get(joint.getName());
         if (linkIndex == -1)
         {

            //create BaseCollider
            bulletMultiBodyRobot.getBtMultiBody()
                                .setBaseCollider(createBulletLinkCollider(bulletMultiBodyRobot,
                                                                          rigidBodyDefinition.getCollisionShapeDefinitions(),
                                                                          rootJoint.getSuccessor().getBodyFixedFrame(),
                                                                          -1,
                                                                          rootJoint.getName()));
         }
         else
         {
            JointDefinition rootJointDefinition = robot.getRobotDefinition().getJointDefinition(joint.getName());
            btMultibodyLink bulletLink = setupBtMultibodyLink(bulletMultiBodyRobot,
                                                              joint,
                                                              rootJointDefinition,
                                                              linkIndex,
                                                              bulletMultiBodyJointParameters.getJointDisableParentCollision());
            bulletLink.m_collider(createBulletLinkCollider(bulletMultiBodyRobot,
                                                           rootJointDefinition.getSuccessor().getCollisionShapeDefinitions(),
                                                           joint.getSuccessor().getBodyFixedFrame(),
                                                           linkIndex,
                                                           joint.getName()));
         }

         //Create LinkColliders for each root joint 
         createBulletLinkColliderChildren(bulletMultiBodyRobot, robot, joint, bulletMultiBodyJointParameters.getJointDisableParentCollision());
      }

      bulletMultiBodyRobot.finalizeMultiDof(bulletMultiBodyParameters, bulletMultiBodyJointParameters);

      return bulletMultiBodyRobot;
   }

   private static void createBulletLinkColliderChildren(BulletMultiBodyRobot bulletMultiBodyRobot,
                                                        Robot robot,
                                                        JointBasics joint,
                                                        boolean disableParentCollision)
   {
      for (JointBasics childJoint : joint.getSuccessor().getChildrenJoints())
      {
         JointDefinition childJointDefinition = robot.getRobotDefinition().getJointDefinition(childJoint.getName());

         int linkIndex = bulletMultiBodyRobot.getJointNameToBulletJointIndexMap().get(childJoint.getName());

         btMultibodyLink btMultibodyLink = setupBtMultibodyLink(bulletMultiBodyRobot, childJoint, childJointDefinition, linkIndex, disableParentCollision);
         btMultibodyLink.m_collider(createBulletLinkCollider(bulletMultiBodyRobot,
                                                             childJointDefinition.getSuccessor().getCollisionShapeDefinitions(),
                                                             childJoint.getSuccessor().getBodyFixedFrame(),
                                                             linkIndex,
                                                             childJoint.getName()));

         createBulletLinkColliderChildren(bulletMultiBodyRobot, robot, childJoint, disableParentCollision);
      }
   }

   private static final btQuaternion rotationFromParentBullet = new btQuaternion();
   private static final Quaternion euclidRotationFromParent = new Quaternion();
   private static final RigidBodyTransform parentLinkCenterOfMassToParentJointBeforeJointFrameTransformEuclid = new RigidBodyTransform();
   private static final btVector3 parentLinkCenterOfMassToParentJointBeforeJointFrameTranslationBullet = new btVector3();
   private static final RigidBodyTransform parentJointAfterFrameToLinkCenterOfMassTransformEuclid = new RigidBodyTransform();
   private static final btVector3 parentJointAfterFrameToLinkCenterOfMassTranslationBullet = new btVector3();

   private static btMultibodyLink setupBtMultibodyLink(BulletMultiBodyRobot bulletMultiBodyRobot,
                                                       JointBasics joint,
                                                       JointDefinition jointDefinition,
                                                       int bulletJointIndex,
                                                       boolean disableParentCollision)
   {

      //      TODO: fix moment of inertia using SVD      
      //      RotationMatrix svd_rotation = new RotationMatrix();
      //      Vector3 linkInertiaDiagonal = decomposeMomentOfInertia(svd_rotation, jointDefinition.getSuccessor());
      btVector3 linkInertiaDiagonal = new btVector3(jointDefinition.getSuccessor().getMomentOfInertia().getM00(),
                                                    jointDefinition.getSuccessor().getMomentOfInertia().getM11(),
                                                    jointDefinition.getSuccessor().getMomentOfInertia().getM22());

      euclidRotationFromParent.set(jointDefinition.getTransformToParent().getRotation());
      euclidRotationFromParent.invert();
      BulletTools.toBullet(euclidRotationFromParent, rotationFromParentBullet);

      joint.getPredecessor().getBodyFixedFrame().getTransformToDesiredFrame(parentLinkCenterOfMassToParentJointBeforeJointFrameTransformEuclid,
                                                                            joint.getFrameBeforeJoint());
      parentLinkCenterOfMassToParentJointBeforeJointFrameTransformEuclid.invert();
      BulletTools.toBullet(parentLinkCenterOfMassToParentJointBeforeJointFrameTransformEuclid.getTranslation(),
                           parentLinkCenterOfMassToParentJointBeforeJointFrameTranslationBullet);

      joint.getFrameAfterJoint().getTransformToDesiredFrame(parentJointAfterFrameToLinkCenterOfMassTransformEuclid, joint.getSuccessor().getBodyFixedFrame());
      parentJointAfterFrameToLinkCenterOfMassTransformEuclid.invert();
      BulletTools.toBullet(parentJointAfterFrameToLinkCenterOfMassTransformEuclid.getTranslation(), parentJointAfterFrameToLinkCenterOfMassTranslationBullet);

      double linkMass = jointDefinition.getSuccessor().getMass();

      if (joint instanceof SimRevoluteJoint)
      {
         RevoluteJointDefinition revoluteJointDefinition = (RevoluteJointDefinition) jointDefinition;

         int parentBulletJointIndex;
         if (joint.getPredecessor().getParentJoint() == null)
            parentBulletJointIndex = -1;
         else
            parentBulletJointIndex = bulletMultiBodyRobot.getJointNameToBulletJointIndexMap().get(joint.getPredecessor().getParentJoint().getName());

         btVector3 jointAxis = new btVector3();
         BulletTools.toBullet(revoluteJointDefinition.getAxis(), jointAxis);

         bulletMultiBodyRobot.getBtMultiBody().setupRevolute(bulletJointIndex,
                                                             linkMass,
                                                             linkInertiaDiagonal,
                                                             parentBulletJointIndex,
                                                             rotationFromParentBullet,
                                                             jointAxis,
                                                             parentLinkCenterOfMassToParentJointBeforeJointFrameTranslationBullet,
                                                             parentJointAfterFrameToLinkCenterOfMassTranslationBullet,
                                                             disableParentCollision);

         btMultiBodyJointLimitConstraint multiBodyJointLimitConstraint = new btMultiBodyJointLimitConstraint(bulletMultiBodyRobot.getBtMultiBody(),
                                                                                                             bulletJointIndex,
                                                                                                             revoluteJointDefinition.getPositionLowerLimit(),
                                                                                                             revoluteJointDefinition.getPositionUpperLimit());

         bulletMultiBodyRobot.addBtMultiBodyConstraint(multiBodyJointLimitConstraint);
      }
      else if (joint instanceof SimPrismaticJoint)
      {
         PrismaticJointDefinition primaticJointDefinition = (PrismaticJointDefinition) jointDefinition;

         int parentBulletJointIndex;
         if (joint.getPredecessor().getParentJoint() == null)
            parentBulletJointIndex = -1;
         else
            parentBulletJointIndex = bulletMultiBodyRobot.getJointNameToBulletJointIndexMap().get(joint.getPredecessor().getParentJoint().getName());

         btVector3 jointAxis = new btVector3();
         BulletTools.toBullet(primaticJointDefinition.getAxis(), jointAxis);

         bulletMultiBodyRobot.getBtMultiBody().setupPrismatic(bulletJointIndex,
                                                              linkMass,
                                                              linkInertiaDiagonal,
                                                              parentBulletJointIndex,
                                                              rotationFromParentBullet,
                                                              jointAxis,
                                                              parentLinkCenterOfMassToParentJointBeforeJointFrameTranslationBullet,
                                                              parentJointAfterFrameToLinkCenterOfMassTranslationBullet,
                                                              disableParentCollision);

         btMultiBodyJointLimitConstraint multiBodyJointLimitConstraint = new btMultiBodyJointLimitConstraint(bulletMultiBodyRobot.getBtMultiBody(),
                                                                                                             bulletJointIndex,
                                                                                                             primaticJointDefinition.getPositionLowerLimit(),
                                                                                                             primaticJointDefinition.getPositionUpperLimit());

         bulletMultiBodyRobot.addBtMultiBodyConstraint(multiBodyJointLimitConstraint);
      }
      else
      {
         throw new UnsupportedOperationException("Unsupported joint: " + joint.getClass().getSimpleName());
      }

      return bulletMultiBodyRobot.getBtMultiBody().getLink(bulletJointIndex);
   }

   //   TODO: fix moment of inertia using SVD   
   //   private static Vector3 decomposeMomentOfInertia(RotationMatrix svd_rotation, RigidBodyDefinition rigidBodyDefinition)
   //   {
   //      Vector3 localInertiaDiagonal = new Vector3();
   //      if (rigidBodyDefinition.getMomentOfInertia().getM01() == 0 && rigidBodyDefinition.getMomentOfInertia().getM02() == 0
   //            && rigidBodyDefinition.getMomentOfInertia().getM21() == 0)
   //      {
   //         localInertiaDiagonal.x = rigidBodyDefinition.getMomentOfInertia().getM00();
   //         localInertiaDiagonal.y = rigidBodyDefinition.getMomentOfInertia().getM11();
   //         localInertiaDiagonal.z = rigidBodyDefinition.getMomentOfInertia().getM22();
   //      }
   //      else {
   //         SingularValueDecomposition3D svd = new SingularValueDecomposition3D();
   //         
   //         if (svd.decompose(rigidBodyDefinition.getMomentOfInertia()))
   //         {
   //            svd_rotation.set(svd.getU());
   //
   //            localInertiaDiagonal.x = svd.getW().getX();
   //            localInertiaDiagonal.y = svd.getW().getY();
   //            localInertiaDiagonal.z = svd.getW().getZ();
   //         }
   //         else
   //         {
   //            LogTools.warn("SVD did not decompose", rigidBodyDefinition.getName());
   //         }
   //      }
   //      if (localInertiaDiagonal.x < 0 || localInertiaDiagonal.x > (localInertiaDiagonal.y + localInertiaDiagonal.z) || localInertiaDiagonal.y < 0
   //            || localInertiaDiagonal.y > (localInertiaDiagonal.x + localInertiaDiagonal.z) || localInertiaDiagonal.z < 0
   //            || localInertiaDiagonal.z > (localInertiaDiagonal.x + localInertiaDiagonal.y))
   //      {
   //         LogTools.warn("Bad inertia tensor properties, setting inertia to zero for link:", rigidBodyDefinition.getName());
   //         localInertiaDiagonal.x = 0.f;
   //         localInertiaDiagonal.y = 0.f;
   //         localInertiaDiagonal.z = 0.f;
   //      }
   //      return localInertiaDiagonal;
   //   }

   private static btMultiBodyLinkCollider createBulletLinkCollider(BulletMultiBodyRobot bulletMultiBodyRobot,
                                                                   List collisionShapeDefinitions,
                                                                   ReferenceFrame linkCenterOfMassFrame,
                                                                   int bulletJointIndex,
                                                                   String jointName)
   {
      BulletMultiBodyLinkCollider bulletMultiBodyLinkCollider = new BulletMultiBodyLinkCollider(bulletMultiBodyRobot.getBtMultiBody(),
                                                                                                bulletJointIndex,
                                                                                                jointName);

      btCompoundShape bulletCompoundShape = new btCompoundShape();
      int collisionGroup = 2;
      int collisionGroupMask = 1 + 2;

      if (collisionShapeDefinitions.size() > 0)
      {
         collisionGroupMask = (int) collisionShapeDefinitions.get(0).getCollisionGroup();
         collisionGroup = (int) collisionShapeDefinitions.get(0).getCollisionMask();
      }

      bulletMultiBodyLinkCollider.setCollisionGroupMask(collisionGroup, collisionGroupMask);

      ArrayList btCollisionShapes = new ArrayList<>();
      for (CollisionShapeDefinition shapeDefinition : collisionShapeDefinitions)
      {
         btCollisionShape bulletCollisionShape = BulletTools.createBulletCollisionShape(shapeDefinition);
         btTransform bulletCollisionShapeLocalTransform = bulletCollisionShapeLocalTransform(shapeDefinition, linkCenterOfMassFrame);
         bulletCompoundShape.addChildShape(bulletCollisionShapeLocalTransform, bulletCollisionShape);
         btCollisionShapes.add(bulletCollisionShape);
      }

      bulletMultiBodyLinkCollider.setCollisionShape(bulletCompoundShape, btCollisionShapes);

      bulletMultiBodyRobot.addBulletMuliBodyLinkCollider(bulletMultiBodyLinkCollider);

      return bulletMultiBodyLinkCollider.getBtMultiBodyLinkCollider();
   }

   private static final btTransform bulletCollisionShapeLocalTransform = new btTransform();
   private static final RigidBodyTransform collisionShapeDefinitionToCenterOfMassFrameTransformEuclid = new RigidBodyTransform();

   public static btTransform bulletCollisionShapeLocalTransform(CollisionShapeDefinition shapeDefinition, ReferenceFrame linkCenterOfMassFrame)
   {
      collisionShapeDefinitionToCenterOfMassFrameTransformEuclid.setAndInvert(linkCenterOfMassFrame.getTransformToParent());
      collisionShapeDefinitionToCenterOfMassFrameTransformEuclid.multiply(new RigidBodyTransform(shapeDefinition.getOriginPose().getRotation(),
                                                                                                 shapeDefinition.getOriginPose().getTranslation()));

      BulletTools.toBullet(collisionShapeDefinitionToCenterOfMassFrameTransformEuclid, bulletCollisionShapeLocalTransform);

      return bulletCollisionShapeLocalTransform;
   }

   private static int countJointsAndCreateIndexMap(JointBasics joint)
   {
      jointNameToBulletJointIndexMap.put(joint.getName(), linkCountingIndex - 1);
      ++linkCountingIndex;

      int numberOfJoints = 1;
      for (JointBasics childrenJoint : joint.getSuccessor().getChildrenJoints())
      {
         numberOfJoints += countJointsAndCreateIndexMap(childrenJoint);
      }
      return numberOfJoints;
   }

}




© 2015 - 2024 Weber Informatics LLC | Privacy Policy