public void setRobotRootJointPositionAndOrientationToMatchFullRobotModel(FloatingJointBasics sixDoFJoint, FloatingJoint floatingJoint) { RigidBodyTransform transform = new RigidBodyTransform(); sixDoFJoint.getJointConfiguration(transform); floatingJoint.setRotationAndTranslation(transform); }
public static void setRandomConfiguration(Random random, Robot robot) { FloatingJoint rootJoint = (FloatingJoint) robot.getRootJoints().get(0); rootJoint.setRotationAndTranslation(EuclidCoreRandomTools.nextRigidBodyTransform(random)); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) { double jointLowerLimit = Math.max(joint.getJointLowerLimit(), -2.0 * Math.PI); double jointUpperLimit = Math.min(joint.getJointUpperLimit(), 2.0 * Math.PI); double q = EuclidCoreRandomTools.nextDouble(random, jointLowerLimit, jointUpperLimit); joint.setQ(q); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingJointBasics sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = new RigidBodyTransform(); sixDoFJoint.getJointConfiguration(transform); floatingJoint.setRotationAndTranslation(transform); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair : oneDoFJointPairList) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } if (rootJointPair != null) { FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingJointBasics sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = new RigidBodyTransform(); sixDoFJoint.getJointConfiguration(transform); floatingJoint.setRotationAndTranslation(transform); } } }
public void attachFaceCube() { Link faceLink = new Link("FaceLink"); Graphics3DObject faceGraphics = new Graphics3DObject(); faceGraphics.scale(new Vector3D(3.0, 3.0, 0.1)); faceGraphics.addModelFile("models/GFE/ihmc/face_cube.dae"); faceLink.setLinkGraphics(faceGraphics); FloatingJoint faceJoint = new FloatingJoint("FaceJoint", "faceJoint" , new Vector3D(), this); faceJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle(new Vector3D(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3D(1.1, 0.0, 1.3))); faceJoint.setLink(faceLink); faceJoint.setDynamic(false); floatingJoint.addJoint(faceJoint); }
public void attachFaceCube() { Link faceLink = new Link("FaceLink"); Graphics3DObject faceGraphics = new Graphics3DObject(); faceGraphics.scale(new Vector3d(3.0, 3.0, 0.1)); faceGraphics.addModelFile("models/GFE/ihmc/face_cube.dae"); faceLink.setLinkGraphics(faceGraphics); FloatingJoint faceJoint = new FloatingJoint("FaceJoint", "faceJoint" , new Vector3d(), this); faceJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3d(1.1, 0.0, 1.3))); faceJoint.setLink(faceLink); faceJoint.setDynamic(false); floatingJoint.addJoint(faceJoint); }
public void attachFaceCube() { Link faceLink = new Link("FaceLink"); Graphics3DObject faceGraphics = new Graphics3DObject(); faceGraphics.scale(new Vector3d(3.0, 3.0, 0.1)); faceGraphics.addModelFile("models/GFE/ihmc/face_cube.dae"); faceLink.setLinkGraphics(faceGraphics); FloatingJoint faceJoint = new FloatingJoint("FaceJoint", "faceJoint" , new Vector3d(), this); faceJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3d(1.1, 0.0, 1.3))); faceJoint.setLink(faceLink); faceJoint.setDynamic(false); floatingJoint.addJoint(faceJoint); }
floatingJoint.setRotationAndTranslation(rootJointTransform); floatingJoint.setLink(polarisLink); floatingJoint.setDynamic(false); wheelJoint.setRotationAndTranslation(PolarisRobot.wheelToCarTransform); wheelJoint.setLink(wheelLink); wheelJoint.setDynamic(false); checkerBoardJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle(new Vector3D(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3D(1.1, -0.5, 1.3))); checkerBoardJoint.setLink(checkerBoardLink); checkerBoardJoint.setDynamic(false);
floatingJoint.setRotationAndTranslation(rootJointTransform); floatingJoint.setLink(polarisLink); floatingJoint.setDynamic(false); wheelJoint.setRotationAndTranslation(PolarisRobot.wheelToCarTransform); wheelJoint.setLink(wheelLink); wheelJoint.setDynamic(false); checkerBoardJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3d(1.1, -0.5, 1.3))); checkerBoardJoint.setLink(checkerBoardLink); checkerBoardJoint.setDynamic(false);
floatingJoint.setRotationAndTranslation(rootJointTransform); floatingJoint.setLink(polarisLink); floatingJoint.setDynamic(false); wheelJoint.setRotationAndTranslation(PolarisRobot.wheelToCarTransform); wheelJoint.setLink(wheelLink); wheelJoint.setDynamic(false); checkerBoardJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3d(1.1, -0.5, 1.3))); checkerBoardJoint.setLink(checkerBoardLink); checkerBoardJoint.setDynamic(false);
public ContactableCylinderRobot(String name, RigidBodyTransform rootJointTransform, double radius, double height, double mass, String modelName) { super(name); rootJointTransformToWorld = rootJointTransform; afterRootJointFrame = new ReferenceFrame("rootJointFrame", worldFrame) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { floatingJoint.getTransformToWorld(transformToParent); } }; frameCylinder = new FrameCylinder3d(afterRootJointFrame, height, radius); link = new Link(name + "Link"); link.setMass(mass); link.setComOffset(new Vector3D(0.0, 0.0, height / 3.0)); Matrix3D inertia = RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(mass, radius, height, Axis.Z); link.setMomentOfInertia(inertia); linkGraphics = new Graphics3DObject(); //linkGraphics.addCoordinateSystem(0.2); linkGraphics.addCylinder(frameCylinder.getHeight(), frameCylinder.getRadius(), YoAppearance.Transparent()); if(modelName!=null) linkGraphics.addModelFile(modelName); link.setLinkGraphics(linkGraphics); floatingJoint = new FloatingJoint(name + "Base", name, new Vector3D(), this); floatingJoint.setRotationAndTranslation(rootJointTransform); floatingJoint.setLink(link); this.addRootJoint(floatingJoint); }
success = success && drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.2); scsRootJoint.setRotationAndTranslation(new RigidBodyTransform(aboveInitialPose.getOrientation(), aboveInitialPose.getPosition()));
RigidBodyTransform pin1ToWorld = new RigidBodyTransform(); pin1.getTransformToWorld(pin1ToWorld); root2.setRotationAndTranslation(pin1ToWorld); root2.setPosition(root2.getQx().getDoubleValue(), root2.getQy().getDoubleValue(), root2.getQz().getDoubleValue()); robot2.update();
floatingJoint.setRotationAndTranslation(rotationAndTranslation);