if(link.getVisuals() != null) transformToModel.multiply(link.getTransformFromModelReferenceFrame()); sdfGraphics3DObject = new SDFGraphics3DObject(link.getCollisions(), resourceDirectories, transformToModel); sdfGraphics3DObject = new SDFGraphics3DObject(link.getVisuals(), resourceDirectories, transformToModel); for(SDFJointHolder joint: link.getChildren())
@Override public ArrayList<CollisionMeshDescription> getCollisionObjects(String name) { // TODO: SDF collision stuff to RobotDescription collision stuff. for(SDFLinkHolder linkHolder : rootLinks) { if(linkHolder.getName().equals(name)) { SDFGraphics3DObject sdfGraphics3DObject = new SDFGraphics3DObject(linkHolder.getCollisions(), resourceDirectories); ArrayList<CollisionMeshDescription> collisionMeshDescriptions = new ArrayList<CollisionMeshDescription>(); //TODO: Figure out and add the collision meshes... return collisionMeshDescriptions; } } SDFGraphics3DObject sdfGraphics3DObject = new SDFGraphics3DObject(joints.get(name).getChildLinkHolder().getCollisions(), resourceDirectories); CollisionMeshDescription collisionMeshDescription = new CollisionMeshDescription(); ArrayList<CollisionMeshDescription> collisionMeshDescriptions = new ArrayList<CollisionMeshDescription>(); //TODO: Figure out and add the collision meshes... return collisionMeshDescriptions; }
private void modifyHokuyoInertia(SDFLinkHolder linkHolder) { linkHolder.getInertia().setM00(0.000401606); // i_xx linkHolder.getInertia().setM01(4.9927e-08); // i_xy linkHolder.getInertia().setM02(1.0997e-05); // i_xz linkHolder.getInertia().setM11(0.00208115); // i_yy linkHolder.getInertia().setM12(-9.8165e-09); // i_yz linkHolder.getInertia().setM22(0.00178402); // i_zz }
@Override public Graphics3DObject getGraphicsObject(String name) { for(SDFLinkHolder linkHolder : rootLinks) { if(linkHolder.getName().equals(name)) { return new SDFGraphics3DObject(linkHolder.getVisuals(), resourceDirectories); } } SDFJointHolder joint = joints.get(name); RigidBodyTransform visualTransform = new RigidBodyTransform(); visualTransform.setRotation(joint.getLinkRotation()); return new SDFGraphics3DObject(joint.getChildLinkHolder().getVisuals(), resourceDirectories, visualTransform); }
private LinkDescription createLinkDescription(SDFLinkHolder link, RigidBodyTransform rotationTransform, boolean useCollisionMeshes) LinkDescription scsLink = new LinkDescription(link.getName()); CollisionMeshDescription collisionMeshDescription = new SDFCollisionMeshDescription(link.getCollisions(), rotationTransform); scsLink.addCollisionMesh(collisionMeshDescription); if (link.getVisuals() != null) linkGraphicsDescription = new SDFGraphics3DObject(link.getVisuals(), resourceDirectories, rotationTransform); PrintTools.warn(this, "Could not load visuals for link " + link.getName() + "! Using an empty LinkGraphicsDescription."); double mass = link.getMass(); Matrix3D inertia = InertiaTools.rotate(rotationTransform, link.getInertia()); Vector3D CoMOffset = new Vector3D(link.getCoMOffset()); if (link.getJoint() != null) if (isJointInNeedOfReducedGains(link.getJoint())) scsLink.setMomentOfInertia(inertia); if (link.getVisuals() != null)
private void checkJointChildren(SDFJointHolder joint) for (Collision collision : link.getCollisions()) link.getTransformFromModelReferenceFrame().transform(gcOffset); addSimulationContactPoint(joint.getName(), gcOffset); link.getTransformFromModelReferenceFrame().transform(gcOffset); boolean assigned = false; for (SDFJointHolder child : link.getChildren())
for (Collision collision : holder.getCollisions()) for (SDFJointHolder joint : holder.getChildren())
Quaternion orientation = new Quaternion(); generalizedSDFRobotModel.getTransformToRoot().get(orientation, offset); FloatingJointDescription rootJointDescription = new FloatingJointDescription(rootLink.getName()); jointDescriptions.put(rootJointDescription.getName(), rootJointDescription); for (SDFJointHolder child : rootLink.getChildren()) for (SDFJointHolder child : rootLink.getChildren())
private void addForceSensorsIncludingDescendants(SDFJointHolder joint, JointNameMap jointNameMap) { addForceSensor(joint, jointNameMap); for (SDFJointHolder child : joint.getChildLinkHolder().getChildren()) { addForceSensorsIncludingDescendants(child, jointNameMap); } }
public SDFCollisionBoxProvider(JaxbSDFLoader loader, String modelName) { GeneralizedSDFRobotModel model = loader.getGeneralizedSDFRobotModel(modelName); SDFLinkHolder rootLink = model.getRootLinks().get(0); recursivelyAddLinks(rootLink.getName(), rootLink); }
RigidBodyTransform linkRotation = new RigidBodyTransform(child.getTransformFromModelReferenceFrame()); linkRotation.setTranslation(0.0, 0.0, 0.0); RigidBodyTransform linkToSensorInZUp = new RigidBodyTransform(); IMUSensorDescription imuMount = new IMUSensorDescription(child.getName() + "_" + sdfSensor.getName(), linkToSensorInZUp);
RigidBodyTransform linkRotation = new RigidBodyTransform(child.getTransformFromModelReferenceFrame()); linkRotation.setTranslation(0.0, 0.0, 0.0); RigidBodyTransform linkToSensor = ModelFileLoaderConversionsHelper.poseToTransform(sensor.getPose());
RigidBodyTransform modelToParentLink = getParentLinkHolder().getTransformFromModelReferenceFrame(); RigidBodyTransform modelToChildLink = getChildLinkHolder().getTransformFromModelReferenceFrame(); SDFJointHolder parentJoint = parentLinkHolder.getJoint(); if (parentJoint != null)
private void checkJointChildren(SDFJointHolder joint) for (Collision collision : link.getCollisions()) link.getTransformFromModelReferenceFrame().transform(gcOffset); addSimulationContactPoint(joint.getName(), gcOffset); link.getTransformFromModelReferenceFrame().transform(gcOffset); boolean assigned = false; for (SDFJointHolder child : link.getChildren())
for (Collision collision : holder.getCollisions()) for (SDFJointHolder joint : holder.getChildren())
public void setupContactPointsFromRobotModel(GeneralizedSDFRobotModel sdf, boolean removeExistingContacts) { if (removeExistingContacts) clearSimulationContactPoints(); for (SDFLinkHolder link : sdf.getRootLinks()) { for (SDFJointHolder joint : link.getChildren()) { checkJointChildren(joint); } } for (RobotSide robotSide : RobotSide.values) { if (!footGroundContactPoints.get(robotSide).isEmpty()) { clearControllerFootContactPoints(); setControllerFootContactPoint(robotSide, footGroundContactPoints.get(robotSide)); } } } }
List<SDFVisual> visuals = linkHolder.getVisuals(); if (visuals != null) switch (linkHolder.getName())
public SDFCollisionBoxProvider(JaxbSDFLoader loader, String modelName) { GeneralizedSDFRobotModel model = loader.getGeneralizedSDFRobotModel(modelName); SDFLinkHolder rootLink = model.getRootLinks().get(0); recursivelyAddLinks(rootLink.getName(), rootLink); }
RigidBodyTransform linkRotation = new RigidBodyTransform(child.getTransformFromModelReferenceFrame()); linkRotation.setTranslation(0.0, 0.0, 0.0); RigidBodyTransform linkToSensorInZUp = new RigidBodyTransform();