public RigidBody getRigidBody(Joint joint) { if (joint instanceof FloatingJoint) { FloatingInverseDynamicsJoint parentSixDoFJoint = floatingToSixDofToJointMap.get(joint); return parentSixDoFJoint.getSuccessor(); } else if (joint instanceof OneDegreeOfFreedomJoint) { OneDoFJoint parentOneDoFJoint = scsToOneDoFJointMap.get(joint); return parentOneDoFJoint.getSuccessor(); } else { throw new RuntimeException(); } }
private void populateRefrenceFrameMap() { originalToLocalFramesMap.put(originalBaseParentJointFrame, localBaseParentJointFrame); originalToLocalFramesMap.put(originalControlFrame, localControlFrame); originalToLocalFramesMap.put(originalBaseFrame, localBaseFrame); for (int i = 0; i < numberOfDoF; i++) { OneDoFJoint originalJoint = originalJoints[i]; RigidBody originalBody = originalJoint.getSuccessor(); OneDoFJoint localJoint = localJoints[i]; RigidBody localBody = localJoint.getSuccessor(); originalToLocalFramesMap.put(originalJoint.getFrameAfterJoint(), localJoint.getFrameAfterJoint()); originalToLocalFramesMap.put(originalJoint.getFrameBeforeJoint(), localJoint.getFrameBeforeJoint()); originalToLocalFramesMap.put(originalBody.getBodyFixedFrame(), localBody.getBodyFixedFrame()); } }
public static List<OneDoFJoint> createRandomTreeRobotWithOneDoFJoints(String prefix, RigidBody rootBody, int numberOfJoints, Random random) { List<OneDoFJoint> tempJoints = new ArrayList<>(); RigidBody predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint joint; if (random.nextBoolean()) joint = addRandomPrismaticJoint(prefix + "Joint" + i, random, predecessor); else joint = addRandomRevoluteJoint(prefix + "Joint" + i, random, predecessor); ScrewTestTools.addRandomRigidBody(prefix + "Body" + i, random, joint); tempJoints.add(joint); predecessor = tempJoints.get(random.nextInt(tempJoints.size())).getSuccessor(); } OneDoFJoint[] jointArray = ScrewTools.filterJoints(ScrewTools.computeSubtreeJoints(rootBody), OneDoFJoint.class); return Arrays.asList(jointArray); }
private LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> generateForceSensorDefinitions( ArrayList<WrenchCalculatorInterface> groundContactPointBasedWrenchCalculators) { LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> forceSensorDefinitions = new LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition>(); for (WrenchCalculatorInterface groundContactPointBasedWrenchCalculator : groundContactPointBasedWrenchCalculators) { OneDegreeOfFreedomJoint forceTorqueSensorJoint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJoint sensorParentJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(forceTorqueSensorJoint); RigidBodyTransform transformFromSensorToParentJoint = new RigidBodyTransform(); groundContactPointBasedWrenchCalculator.getTransformToParentJoint(transformFromSensorToParentJoint); ForceSensorDefinition sensorDefinition = new ForceSensorDefinition(groundContactPointBasedWrenchCalculator.getName(), sensorParentJoint.getSuccessor(), transformFromSensorToParentJoint); forceSensorDefinitions.put(groundContactPointBasedWrenchCalculator, sensorDefinition); } return forceSensorDefinitions; }
private void computeDesiredOrientation(FrameOrientation desiredOrientationToPack) { if (stateMachine.getCurrentStateEnum() == HeadControlMode.TASKSPACE) { taskspaceHeadControlState.getDesiredOrientation(initialOrientation); } else { updateJointsAtDesiredPosition(); ReferenceFrame desiredEndEffectorFrame = jointsAtDesiredPosition[jointsAtDesiredPosition.length - 1].getSuccessor().getBodyFixedFrame(); desiredOrientationToPack.setToZero(desiredEndEffectorFrame); } }
private void computeDesiredOrientation(FrameOrientation desiredOrientationToPack) { if (stateMachine.getCurrentStateEnum() == ChestControlMode.TASKSPACE) { taskspaceChestControlState.getDesiredOrientation(initialOrientation); } else { updateJointsAtDesiredPosition(); ReferenceFrame desiredEndEffectorFrame = jointsAtDesiredPosition[jointsAtDesiredPosition.length - 1].getSuccessor().getBodyFixedFrame(); desiredOrientationToPack.setToZero(desiredEndEffectorFrame); } if (initialOrientation.containsNaN()) initialOrientation.setToZero(chestFrame); }
RigidBody spinePitchBody = fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH).getSuccessor(); spinePitchCenterOfMassCalculator = createCenterOfMassCalculatorInJointZUpFrame(spinePitchBody, true); spinePitchCoMInZUpFrame = new YoFramePoint("spinePitchCoMInZUpFrame", spinePitchCenterOfMassCalculator.getDesiredFrame(), registry); RigidBody leftHipPitchBody = fullRobotModel.getLegJoint(RobotSide.LEFT, LegJointName.HIP_PITCH).getSuccessor(); leftHipPitchCenterOfMassCalculator = createCenterOfMassCalculatorInJointZUpFrame(leftHipPitchBody, true); leftHipPitchCoMInZUpFrame = new YoFramePoint("leftHipPitchCoMInZUpFrame", leftHipPitchCenterOfMassCalculator.getDesiredFrame(), registry); RigidBody rightHipPitchBody = fullRobotModel.getLegJoint(RobotSide.RIGHT, LegJointName.HIP_PITCH).getSuccessor(); rightHipPitchCenterOfMassCalculator = createCenterOfMassCalculatorInJointZUpFrame(rightHipPitchBody, true); rightHipPitchCoMInZUpFrame = new YoFramePoint("rightHipPitchCoMInZUpFrame", rightHipPitchCenterOfMassCalculator.getDesiredFrame(), registry); RigidBody leftKneeBody = fullRobotModel.getLegJoint(RobotSide.LEFT, LegJointName.KNEE_PITCH).getSuccessor(); leftKneeCenterOfMassCalculator = createCenterOfMassCalculatorInJointZUpFrame(leftKneeBody, true); leftKneeCoMInZUpFrame = new YoFramePoint("leftKneeCoMInZUpFrame", leftKneeCenterOfMassCalculator.getDesiredFrame(), registry); RigidBody rightKneeBody = fullRobotModel.getLegJoint(RobotSide.RIGHT, LegJointName.KNEE_PITCH).getSuccessor(); rightKneeCenterOfMassCalculator = createCenterOfMassCalculatorInJointZUpFrame(rightKneeBody, true); rightKneeCoMInZUpFrame = new YoFramePoint("rightKneeCoMInZUpFrame", rightKneeCenterOfMassCalculator.getDesiredFrame(), registry);
final RigidBody shin = fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH).getSuccessor(); createMassAndCoMOffsetCorruptors(namePrefix, shinName, shin); continue; RigidBody rigidBody = armJoint.getSuccessor(); createMassAndCoMOffsetCorruptors(namePrefix, rigidBody.getName(), rigidBody);
public ReachabilitySphereMapCalculator(OneDoFJoint[] robotArmJoints, SimulationConstructionSet scs) { this.robotArmJoints = robotArmJoints; this.scs = scs; lastJoint = robotArmJoints[robotArmJoints.length - 1]; jacobian = new GeometricJacobian(robotArmJoints, lastJoint.getSuccessor().getBodyFixedFrame()); int maxIterations = 500; spatialInverseKinematicsCalculator = createNumericalInverseKinematicsCalculator(jacobian, maxIterations, true); linearInverseKinematicsCalculator = createNumericalInverseKinematicsCalculator(jacobian, maxIterations, false); ReferenceFrame frameBeforeRootJoint = robotArmJoints[0].getFrameBeforeJoint(); RigidBodyTransform gridTransformToParent = new RigidBodyTransform(new AxisAngle4d(), new Vector3d(gridSizeInNumberOfVoxels * voxelSize / 3.0, 0.0, 0.0)); ReferenceFrame gridFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("gridFrame", frameBeforeRootJoint, gridTransformToParent); Graphics3DObject gridFrameViz = new Graphics3DObject(); gridFrameViz.transform(gridFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame())); gridFrameViz.addCoordinateSystem(1.0, YoAppearance.Blue()); scs.addStaticLinkGraphics(gridFrameViz); sphereVoxelShape = new SphereVoxelShape(gridFrame, voxelSize, numberOfRays, numberOfRotationsAroundRay, SphereVoxelType.graspOrigin); voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, gridSizeInNumberOfVoxels, voxelSize); scs.addYoVariableRegistry(registry); }
double acceleration = desiredAcceleration.get(0, 0); OneDoFJoint joint = (OneDoFJoint) command.getJoint(i); RigidBody controlledBody = joint.getSuccessor();
RigidBody lowerArmBody = elbowJoint.getSuccessor(); upperArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(upperArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian upperArmJacobian = new GeometricJacobian(upperArmJointsClone.get(robotSide), upperArmJointsClone.get(robotSide)[upperArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForUpperArm = new NumericalInverseKinematicsCalculator(upperArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); lowerArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(lowerArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian lowerArmJacobian = new GeometricJacobian(lowerArmJointsClone.get(robotSide), lowerArmJointsClone.get(robotSide)[lowerArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForLowerArm = new NumericalInverseKinematicsCalculator(lowerArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar);
double subtreeMass = TotalMassCalculator.computeSubTreeMass(highLevelControllerOutputJoint.getSuccessor()); jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass);
double subtreeMass = TotalMassCalculator.computeSubTreeMass(highLevelControllerOutputJoint.getSuccessor()); jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass);
originalControlFrame = endEffector.getBodyFixedFrame(); localEndEffectorFrame = localJoints[numberOfDoF - 1].getSuccessor().getBodyFixedFrame(); localControlFrame = createLocalControlFrame(localEndEffectorFrame, originalEndEffectorFrame);
robotJoints = ScrewTools.filterJoints(ScrewTools.createJointPath(base, foot), OneDoFJoint.class); ikJoints = ScrewTools.filterJoints(ScrewTools.cloneJointPath(robotJoints), OneDoFJoint.class); jacobian = new GeometricJacobian(ikJoints, ikJoints[ikJoints.length - 1].getSuccessor().getBodyFixedFrame());