private List<SDFVisual> createSDFVisual(Link scsLink) { if (scsLink.getName().contains("body")) System.out.println(); List<SDFVisual> sdfVisuals = new ArrayList<>(); ArrayList<Graphics3DPrimitiveInstruction> graphics3dInstructions = scsLink.getLinkGraphics().getGraphics3DInstructions(); RigidBodyTransform parentJointTransformToWorld = new RigidBodyTransform(); Joint parentJoint = scsLink.getParentJoint(); if (scsRobot.getRootJoints().contains(parentJoint)) parentJoint.getTransformToWorld(parentJointTransformToWorld); SDFVisual sdfVisual = createSDFVisual(parentJointTransformToWorld, graphics3dInstructions); if (sdfVisual != null) sdfVisuals.add(sdfVisual); return sdfVisuals; }
private List<SDFVisual> createSDFVisual(Link scsLink) { if (scsLink.getName().contains("body")) System.out.println(); List<SDFVisual> sdfVisuals = new ArrayList<>(); ArrayList<Graphics3DPrimitiveInstruction> graphics3dInstructions = scsLink.getLinkGraphics().getGraphics3DInstructions(); RigidBodyTransform parentJointTransformToWorld = new RigidBodyTransform(); Joint parentJoint = scsLink.getParentJoint(); if (scsRobot.getRootJoints().contains(parentJoint)) parentJoint.getTransformToWorld(parentJointTransformToWorld); SDFVisual sdfVisual = createSDFVisual(parentJointTransformToWorld, graphics3dInstructions); if (sdfVisual != null) sdfVisuals.add(sdfVisual); return sdfVisuals; }
private void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
private void addForcePoint() { externalForcePoint1 = new ExternalForcePoint("hand1ForcePoint", scsRobotArm.getLink("hand1").getComOffset(), scsRobotArm); externalForcePoint2 = new ExternalForcePoint("hand2ForcePoint", scsRobotArm.getLink("hand2").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand1").getParentJoint().addExternalForcePoint(externalForcePoint1); scsRobotArm.getLink("hand2").getParentJoint().addExternalForcePoint(externalForcePoint2); }
private void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
private void addForcePoint() { externalForcePoint1 = new ExternalForcePoint("hand1ForcePoint", scsRobotArm.getLink("hand1").getComOffset(), scsRobotArm); externalForcePoint2 = new ExternalForcePoint("hand2ForcePoint", scsRobotArm.getLink("hand2").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand1").getParentJoint().addExternalForcePoint(externalForcePoint1); scsRobotArm.getLink("hand2").getParentJoint().addExternalForcePoint(externalForcePoint2); }
private Inertial createSDFInertial(Link scsLink) { Inertial sdfInertial = new Inertial(); sdfInertial.setInertia(createSDFInertia(scsLink)); sdfInertial.setMass(String.valueOf(scsLink.getMass())); RigidBodyTransform comOffsetInWorld = new RigidBodyTransform(); scsLink.getParentJoint().getTransformToWorld(comOffsetInWorld); Vector3D com = new Vector3D(); scsLink.getComOffset(com); RigidBodyTransform comOffset = TransformTools.createTranslationTransform(com); comOffsetInWorld.multiply(comOffset); sdfInertial.setPose(getPoseFromTransform3D(comOffsetInWorld)); return sdfInertial; }
private Inertial createSDFInertial(Link scsLink) { Inertial sdfInertial = new Inertial(); sdfInertial.setInertia(createSDFInertia(scsLink)); sdfInertial.setMass(String.valueOf(scsLink.getMass())); RigidBodyTransform comOffsetInWorld = new RigidBodyTransform(); scsLink.getParentJoint().getTransformToWorld(comOffsetInWorld); Vector3d com = new Vector3d(); scsLink.getComOffset(com); RigidBodyTransform comOffset = TransformTools.createTranslationTransform(com); comOffsetInWorld.multiply(comOffset); sdfInertial.setPose(getPoseFromTransform3D(comOffsetInWorld)); return sdfInertial; }