public FullInverseDynamicsStructure(RigidBody elevator, RigidBody estimationLink, FloatingInverseDynamicsJoint rootInverseDynamicsJoint) { this.elevator = elevator; this.rootJoint = rootInverseDynamicsJoint; twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), elevator); spatialAccelerationCalculator = new SpatialAccelerationCalculator(elevator, twistCalculator, 0.0, false); this.estimationLink = estimationLink; }
public static void main(String[] args) { Random random = new Random(); int numberOfJoints = 5; List<RevoluteJoint> randomChainRobot = MultiBodySystemRandomTools.nextRevoluteJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, randomChainRobot.get(0).getPredecessor()); Twist dummyTwist = new Twist(); while (true) { twistCalculator.compute(); for (int i = 0; i < 100; i++) twistCalculator.getTwistOfBody(randomChainRobot.get(random.nextInt(numberOfJoints)).getSuccessor(), dummyTwist); } } }
private void initialize() { hasBeenInitialized = true; TwistCalculator twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), toolBody); boolean doVelocityTerms = true; boolean useDesireds = false; SpatialAccelerationCalculator spatialAccelerationCalculator = new SpatialAccelerationCalculator(toolBody, elevatorFrame, ScrewTools.createGravitationalSpatialAcceleration(fullRobotModel.getElevator(), gravity), twistCalculator, doVelocityTerms, useDesireds); ArrayList<InverseDynamicsJoint> jointsToIgnore = new ArrayList<InverseDynamicsJoint>(); jointsToIgnore.addAll(twistCalculator.getRootBody().getChildrenJoints()); jointsToIgnore.remove(toolJoint); inverseDynamicsCalculator = new InverseDynamicsCalculator(ReferenceFrame.getWorldFrame(), new LinkedHashMap<RigidBody, Wrench>(), jointsToIgnore, spatialAccelerationCalculator, twistCalculator, doVelocityTerms); }
List<OneDoFJointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneOneDoFJointKinematicChain(joints.toArray(new OneDoFJointBasics[numberOfJoints]))); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor());
List<OneDoFJointBasics> jointsInFuture = SubtreeStreams.fromChildren(OneDoFJointBasics.class, rootBodyInFuture).collect(Collectors.toList()); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, rootBody);
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { twistCalculator = new TwistCalculator(inertialFrame, rootBody); LinkedHashMap<RigidBody, Wrench> zeroExternalWrench = new LinkedHashMap<RigidBody, Wrench>(); ArrayList<InverseDynamicsJoint> zeroJointToIgnore = new ArrayList<InverseDynamicsJoint>(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(inertialFrame, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true, twistCalculator); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (InverseDynamicsJoint joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithPrismaticChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<PrismaticJoint> joints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
private void setupTest() { YoVariableRegistry registry = new YoVariableRegistry(getClass().getSimpleName()); YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry(); double controlDT = 0.005; fullHumanoidRobotModel = new FullRobotModelTestTools.RandomFullHumanoidRobotModel(random); fullHumanoidRobotModel.updateFrames(); CommonHumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullHumanoidRobotModel); TwistCalculator twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), fullHumanoidRobotModel.getElevator()); ControllerCoreOptimizationSettings momentumOptimizationSettings = new GeneralMomentumOptimizationSettings(); ArrayList<ContactablePlaneBody> contactablePlaneBodies = new ArrayList<>(); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics footBody = fullHumanoidRobotModel.getFoot(robotSide); ReferenceFrame soleFrame = fullHumanoidRobotModel.getSoleFrame(robotSide); contactablePlaneBodies.add(ContactablePlaneBodyTools.createTypicalContactablePlaneBodyForTests(footBody, soleFrame)); } JointBasics[] jointsToOptimizeFor = HighLevelHumanoidControllerToolbox.computeJointsToOptimizeFor(fullHumanoidRobotModel, new JointBasics[0]); FloatingJointBasics rootJoint = fullHumanoidRobotModel.getRootJoint(); ReferenceFrame centerOfMassFrame = referenceFrames.getCenterOfMassFrame(); toolbox = new WholeBodyControlCoreToolbox(controlDT, gravityZ, rootJoint, jointsToOptimizeFor, centerOfMassFrame, momentumOptimizationSettings, yoGraphicsListRegistry, registry); toolbox.setupForInverseDynamicsSolver(contactablePlaneBodies); jointIndexHandler = toolbox.getJointIndexHandler(); coriolisMatrixCalculator = new GravityCoriolisExternalWrenchMatrixCalculator(toolbox.getRootBody(), new ArrayList<>(), toolbox.getGravityZ()); degreesOfFreedom = jointIndexHandler.getNumberOfDoFs(); floatingBaseDoFs = fullHumanoidRobotModel.getRootJoint().getDegreesOfFreedom(); bodyDoFs = degreesOfFreedom - floatingBaseDoFs; }
public DesiredCoMAccelerationsFromRobotStealerController(ReferenceFrame estimationFrame, double comAccelerationProcessNoiseStandardDeviation, double angularAccelerationProcessNoiseStandardDeviation, InverseDynamicsJointsFromSCSRobotGenerator generator, Joint estimationJoint, double controlDT) { this.estimationFrame = estimationFrame; this.comAccelerationProcessNoiseStandardDeviation = comAccelerationProcessNoiseStandardDeviation; this.angularAccelerationProcessNoiseStandardDeviation = angularAccelerationProcessNoiseStandardDeviation; this.controlDT = controlDT; this.generator = generator; RigidBody elevator = generator.getElevator(); perfectTwistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), elevator); perfectSpatialAccelerationCalculator = new SpatialAccelerationCalculator(elevator, perfectTwistCalculator, 0.0, false); perfectCenterOfMassCalculator = new CenterOfMassCalculator(elevator, ReferenceFrame.getWorldFrame()); perfectCenterOfMassJacobian = new CenterOfMassJacobian(elevator); perfectCenterOfMassAccelerationCalculator = new CenterOfMassAccelerationCalculator(elevator, perfectSpatialAccelerationCalculator); centerOfMassAccelerationFromFullRobotModelStealer = new CenterOfMassAccelerationFromFullRobotModelStealer(elevator, perfectSpatialAccelerationCalculator); angularAccelerationFromRobotStealer = new AngularAccelerationFromRobotStealer(estimationJoint); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testWithChainComposedOfPrismaticJoints() throws Exception { Random random = new Random(234234L); int numberOfJoints = 20; List<PrismaticJoint> prismaticJoints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, prismaticJoints.get(random.nextInt(numberOfJoints)).getPredecessor()); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -10.0, 10.0, prismaticJoints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -10.0, 10.0, prismaticJoints); twistCalculator.compute(); FrameVector3D cumulatedLinearVelocity = new FrameVector3D(worldFrame); for (PrismaticJoint joint : prismaticJoints) { RigidBodyBasics body = joint.getSuccessor(); Twist actualTwist = new Twist(); twistCalculator.getTwistOfBody(body, actualTwist); ReferenceFrame bodyFrame = body.getBodyFixedFrame(); Twist expectedTwist = new Twist(bodyFrame, worldFrame, bodyFrame); FrameVector3D jointAxis = new FrameVector3D(joint.getJointAxis()); cumulatedLinearVelocity.changeFrame(jointAxis.getReferenceFrame()); cumulatedLinearVelocity.scaleAdd(joint.getQd(), jointAxis, cumulatedLinearVelocity); cumulatedLinearVelocity.changeFrame(bodyFrame); expectedTwist.getLinearPart().set(cumulatedLinearVelocity); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-12); } } }
referenceFrames = new HumanoidReferenceFrames(desiredFullRobotModel); RigidBody elevator = desiredFullRobotModel.getElevator(); twistCalculator = new TwistCalculator(worldFrame, elevator);
CommonHumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullHumanoidRobotModel); TwistCalculator twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), fullHumanoidRobotModel.getElevator());
int numberOfJoints = 20; List<RevoluteJoint> revoluteJoints = MultiBodySystemRandomTools.nextRevoluteJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, revoluteJoints.get(random.nextInt(numberOfJoints)).getPredecessor());
int numberOfJoints = 100; List<PrismaticJoint> prismaticJoints = MultiBodySystemRandomTools.nextPrismaticJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, prismaticJoints.get(random.nextInt(numberOfJoints)).getPredecessor());
List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist();
int numberOfJoints = 100; List<RevoluteJoint> revoluteJoints = MultiBodySystemRandomTools.nextRevoluteJointTree(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, revoluteJoints.get(random.nextInt(numberOfJoints)).getPredecessor());
public CentroidalMomentumRateADotVTerm(RigidBody rootBody, ReferenceFrame centerOfMassFrame, CentroidalMomentumMatrix centroidalMomentumMatrix, double robotMass, DenseMatrix64F v) { this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.robotMass = robotMass; this.centerOfMassFrame = centerOfMassFrame; this.rootBody = rootBody; this.v = v; this.twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), rootBody); this.centroidalMomentumMatrix = centroidalMomentumMatrix; this.rootAcceleration = new SpatialAccelerationVector(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame()); this.spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame(), this.rootAcceleration, this.twistCalculator, true, false, false); this.comTwist = new Twist(centerOfMassFrame, rootBody.getBodyFixedFrame(), centerOfMassFrame); comTwist.setToZero(); this.comVelocityVector = new Vector3d(); this.aDotV = new DenseMatrix64F(6, 1); this.tempSpatialAcceleration = new SpatialAccelerationVector(); this.tempMomentum = new Momentum(); this.tempTwist = new Twist(); this.tempCoMTwist = new Twist(); this.tempVector = new Vector3d(0, 0, 0); this.leftSide = new Momentum(centerOfMassFrame); }
this.rigidBody = rigidBody; this.imuFrame = imuFrame; this.twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), rootBody); this.spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame()); spatialAccelerationCalculator.setRootAcceleration(rootAcceleration);
this.twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), rootBody);