public String toString() { return world.toString(); }
private SDFSensor createSDFSimulatedIMU(Link scsLink) { SDFSensor sdfSensor = new SDFSensor(); sdfSensor.setName(sdfSimulatedIMUName); sdfSensor.setType("imu"); IMU sdfSimulatedIMU = new IMU(); IMUNoise imuNoise = new IMUNoise(); NoiseParameters noiseParameters = new NoiseParameters(); noiseParameters.setBias_mean("0"); noiseParameters.setBias_stddev("0"); noiseParameters.setMean("0"); noiseParameters.setStddev("0"); imuNoise.setAccel(noiseParameters); imuNoise.setRate(noiseParameters); imuNoise.setType("gaussian"); sdfSimulatedIMU.setNoise(imuNoise); sdfSensor.setImu(sdfSimulatedIMU); sdfSensor.setPose("0 0 0 0 0 0"); return sdfSensor; }
public SDFLinkHolder(SDFLink sdfLink) name = ModelFileLoaderConversionsHelper.sanitizeJointName(sdfLink.getName()); transformToModelReferenceFrame = ModelFileLoaderConversionsHelper.poseToTransform(sdfLink.getPose()); if(sdfLink.getInertial() != null) inertialFrameWithRespectToLinkFrame = ModelFileLoaderConversionsHelper.poseToTransform(sdfLink.getInertial().getPose()); mass = Double.parseDouble(sdfLink.getInertial().getMass()); inertia = ModelFileLoaderConversionsHelper.sdfInertiaToMatrix3d(sdfLink.getInertial().getInertia()); visuals = sdfLink.getVisuals(); sensors = sdfLink.getSensors(); if(sdfLink.getCollisions() != null) collisions = sdfLink.getCollisions(); if((sdfLink.getCollisions().get(0) != null) && (sdfLink.getCollisions().get(0).getSurface() != null) && (sdfLink.getCollisions().get(0).getSurface().getContact() != null) && (sdfLink.getCollisions().get(0).getSurface().getContact().getOde() != null)) if (sdfLink.getCollisions().get(0).getSurface().getContact().getOde().getKp() != null) contactKp = Double.parseDouble(sdfLink.getCollisions().get(0).getSurface().getContact().getOde().getKp()); if (sdfLink.getCollisions().get(0).getSurface().getContact().getOde().getKd() != null) contactKd = Double.parseDouble(sdfLink.getCollisions().get(0).getSurface().getContact().getOde().getKd()); if (sdfLink.getCollisions().get(0).getSurface().getContact().getOde().getMaxVel() != null) contactMaxVel = Double.parseDouble(sdfLink.getCollisions().get(0).getSurface().getContact().getOde().getMaxVel());
private SDFInertia createSDFInertia(Link scsLink) { SDFInertia sdfInertia = new SDFInertia(); Matrix3d scsInertia = new Matrix3d(); scsLink.getMomentOfInertia(scsInertia); sdfInertia.setIxx(String.valueOf(scsInertia.getM00())); sdfInertia.setIyy(String.valueOf(scsInertia.getM11())); sdfInertia.setIzz(String.valueOf(scsInertia.getM22())); sdfInertia.setIxy(String.valueOf(scsInertia.getM01())); sdfInertia.setIxz(String.valueOf(scsInertia.getM02())); sdfInertia.setIyz(String.valueOf(scsInertia.getM12())); return sdfInertia; }
private final Limit createJointLimit(OneDegreeOfFreedomJoint scsJoint) { Limit sdfJointLimit = new Limit(); String effort = String.valueOf(scsJoint.getTorqueLimit()); sdfJointLimit.setEffort(effort); String velocity = String.valueOf(scsJoint.getVelocityLimit()); sdfJointLimit.setVelocity(velocity); String lower = String.valueOf(scsJoint.getJointLowerLimit()); sdfJointLimit.setLower(lower); String upper = String.valueOf(scsJoint.getJointUpperLimit()); sdfJointLimit.setUpper(upper); double jointRange = scsJoint.getJointUpperLimit() - scsJoint.getJointLowerLimit(); if (jointRange == 0.0) PrintTools.debug(this, scsJoint.getName() + " upper joint limit equals lower joint limit!"); return sdfJointLimit; }
private SDFLink createSDFLink(Link scsLink, boolean addSimulatedIMU) { SDFLink sdfLink = new SDFLink(); sdfLink.setInertial(createSDFInertial(scsLink)); sdfLink.setName(scsLink.getName()); String pose = "0 0 0 0 0 0"; sdfLink.setPose(pose); sdfLink.setVisuals(createSDFVisual(scsLink)); if (addSimulatedIMU) { List<SDFSensor> sdfSensors = new ArrayList<>(); sdfSensors.add(createSDFSimulatedIMU(scsLink)); sdfLink.setSensors(sdfSensors); } return sdfLink; }
private Axis createSDFJointAxis(OneDegreeOfFreedomJoint scsJoint) { Axis sdfJointAxis = new Axis(); Vector3d scsJointAxis = new Vector3d(); scsJoint.getJointAxis(scsJointAxis); String xyz = String.valueOf(scsJointAxis.getX()) + " " + String.valueOf(scsJointAxis.getY()) + " " + String.valueOf(scsJointAxis.getZ()); sdfJointAxis.setXyz(xyz); sdfJointAxis.setDynamics(createJointDynamics(scsJoint)); sdfJointAxis.setLimit(createJointLimit(scsJoint)); return sdfJointAxis; }
SDFGeometry geometry = sdfVisual.getGeometry(); if (geometry == null) continue; SDFGeometry.Mesh mesh = geometry.getMesh(); if (mesh == null) continue; String meshUri = mesh.getUri(); if (meshUri.contains("meshes")) mesh.setUri(replacedURI);
private Dynamics createJointDynamics(OneDegreeOfFreedomJoint scsJoint) { Dynamics sdfJointDynamics = new Dynamics(); String damping = String.valueOf(scsJoint.getDamping()); sdfJointDynamics.setDamping(damping); String friction = String.valueOf(scsJoint.getJointStiction()); sdfJointDynamics.setFriction(friction); return sdfJointDynamics; }
public static Matrix3D sdfInertiaToMatrix3d(SDFInertia sdfInertia) { Matrix3D inertia = new Matrix3D(); if(sdfInertia != null) { double ixx = Double.parseDouble(sdfInertia.getIxx()); double ixy = Double.parseDouble(sdfInertia.getIxy()); double ixz = Double.parseDouble(sdfInertia.getIxz()); double iyy = Double.parseDouble(sdfInertia.getIyy()); double iyz = Double.parseDouble(sdfInertia.getIyz()); double izz = Double.parseDouble(sdfInertia.getIzz()); inertia.setM00(ixx); inertia.setM01(ixy); inertia.setM02(ixz); inertia.setM10(ixy); inertia.setM11(iyy); inertia.setM12(iyz); inertia.setM20(ixz); inertia.setM21(iyz); inertia.setM22(izz); } return inertia; }
public JaxbSDFLoader(InputStream inputStream, List<String> resourceDirectories, SDFDescriptionMutator mutator) throws JAXBException, FileNotFoundException { if (inputStream == null) { throw new RuntimeException("inputStream is null"); } JAXBContext context = JAXBContext.newInstance(SDFRoot.class); Unmarshaller um = context.createUnmarshaller(); SDFRoot sdfRoot = (SDFRoot) um.unmarshal(inputStream); List<SDFModel> models; if (sdfRoot.getWorld() != null) { models = sdfRoot.getWorld().getModels(); if (sdfRoot.getWorld().getRoads() != null) { roads.addAll(sdfRoot.getWorld().getRoads()); } } else { models = sdfRoot.getModels(); } for (SDFModel modelInstance : models) { final String modelName = modelInstance.getName(); generalizedSDFRobotModels.put(modelName, new GeneralizedSDFRobotModel(modelName, modelInstance, resourceDirectories, mutator)); } }
public void writeSDFRobotDescriptionFile() throws PropertyException, JAXBException { File output = new File(sdfFilePath); models.add(createSDFModel()); sdfRobot.setModels(models); marshaller.setProperty(Marshaller.JAXB_FORMATTED_OUTPUT, Boolean.TRUE); marshaller.marshal(sdfRobot, output); }
public SDFWorldLoader(InputStream inputStream, List<String> resourceDirectories) throws FileNotFoundException, JAXBException { jaxbSDFLoader = new JaxbSDFLoader(inputStream, resourceDirectories, null); for (GeneralizedSDFRobotModel generalizedSDFRobotModel : jaxbSDFLoader.getGeneralizedSDFRobotModels()) { String name = generalizedSDFRobotModel.getName(); visuals.put(name, new SDFModelVisual(generalizedSDFRobotModel)); } for (Road road : jaxbSDFLoader.getRoads()) { visuals.put(road.getName(), new SDFRoadVisual(road)); } }
private void addSensors(JointDescription scsJoint, SDFLinkHolder child) { if (child.getSensors() != null) { for (SDFSensor sensor : child.getSensors()) { switch (sensor.getType()) { case "camera": case "multicamera": case "depth": addCameraMounts(sensor, scsJoint, child); break; case "imu": addIMUMounts(sensor, scsJoint, child); break; case "gpu_ray": case "ray": addLidarMounts(sensor, scsJoint, child); break; } } } }
private SDFSensor createSDFSimulatedIMU(Link scsLink) { SDFSensor sdfSensor = new SDFSensor(); sdfSensor.setName(sdfSimulatedIMUName); sdfSensor.setType("imu"); IMU sdfSimulatedIMU = new IMU(); IMUNoise imuNoise = new IMUNoise(); NoiseParameters noiseParameters = new NoiseParameters(); noiseParameters.setBias_mean("0"); noiseParameters.setBias_stddev("0"); noiseParameters.setMean("0"); noiseParameters.setStddev("0"); imuNoise.setAccel(noiseParameters); imuNoise.setRate(noiseParameters); imuNoise.setType("gaussian"); sdfSimulatedIMU.setNoise(imuNoise); sdfSensor.setImu(sdfSimulatedIMU); sdfSensor.setPose("0 0 0 0 0 0"); return sdfSensor; }
private SDFInertia createSDFInertia(Link scsLink) { SDFInertia sdfInertia = new SDFInertia(); Matrix3D scsInertia = new Matrix3D(); scsLink.getMomentOfInertia(scsInertia); sdfInertia.setIxx(String.valueOf(scsInertia.getM00())); sdfInertia.setIyy(String.valueOf(scsInertia.getM11())); sdfInertia.setIzz(String.valueOf(scsInertia.getM22())); sdfInertia.setIxy(String.valueOf(scsInertia.getM01())); sdfInertia.setIxz(String.valueOf(scsInertia.getM02())); sdfInertia.setIyz(String.valueOf(scsInertia.getM12())); return sdfInertia; }
private final Limit createJointLimit(OneDegreeOfFreedomJoint scsJoint) { Limit sdfJointLimit = new Limit(); String effort = String.valueOf(scsJoint.getTorqueLimit()); sdfJointLimit.setEffort(effort); String velocity = String.valueOf(scsJoint.getVelocityLimit()); sdfJointLimit.setVelocity(velocity); String lower = String.valueOf(scsJoint.getJointLowerLimit()); sdfJointLimit.setLower(lower); String upper = String.valueOf(scsJoint.getJointUpperLimit()); sdfJointLimit.setUpper(upper); double jointRange = scsJoint.getJointUpperLimit() - scsJoint.getJointLowerLimit(); if (jointRange == 0.0) PrintTools.debug(this, scsJoint.getName() + " upper joint limit equals lower joint limit!"); return sdfJointLimit; }
private Axis createSDFJointAxis(OneDegreeOfFreedomJoint scsJoint) { Axis sdfJointAxis = new Axis(); Vector3D scsJointAxis = new Vector3D(); scsJoint.getJointAxis(scsJointAxis); String xyz = String.valueOf(scsJointAxis.getX()) + " " + String.valueOf(scsJointAxis.getY()) + " " + String.valueOf(scsJointAxis.getZ()); sdfJointAxis.setXyz(xyz); sdfJointAxis.setDynamics(createJointDynamics(scsJoint)); sdfJointAxis.setLimit(createJointLimit(scsJoint)); return sdfJointAxis; }
private Dynamics createJointDynamics(OneDegreeOfFreedomJoint scsJoint) { Dynamics sdfJointDynamics = new Dynamics(); String damping = String.valueOf(scsJoint.getDamping()); sdfJointDynamics.setDamping(damping); String friction = String.valueOf(scsJoint.getJointStiction()); sdfJointDynamics.setFriction(friction); return sdfJointDynamics; }
public void writeSDFRobotDescriptionFile() throws PropertyException, JAXBException { File output = new File(sdfFilePath); models.add(createSDFModel()); sdfRobot.setModels(models); marshaller.setProperty(Marshaller.JAXB_FORMATTED_OUTPUT, Boolean.TRUE); marshaller.marshal(sdfRobot, output); }