public KinematicsToolboxController(CommandInputManager commandInputManager, StatusMessageOutputManager statusOutputManager, FloatingJointBasics rootJoint, OneDoFJointBasics[] oneDoFJoints, Collection<RigidBodyBasics> controllableRigidBodies, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { super(statusOutputManager, parentRegistry); this.commandInputManager = commandInputManager; this.rootJoint = rootJoint; this.oneDoFJoints = oneDoFJoints; this.yoGraphicsListRegistry = yoGraphicsListRegistry; // This will find the root body without using rootJoint so it can be null. rootBody = MultiBodySystemTools.getRootBody(oneDoFJoints[0].getPredecessor()); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", worldFrame, rootBody); Arrays.stream(oneDoFJoints).forEach(joint -> jointHashCodeMap.put(joint.hashCode(), joint)); controllerCore = createControllerCore(controllableRigidBodies); feedbackControllerDataHolder = controllerCore.getWholeBodyFeedbackControllerDataHolder(); inverseKinematicsSolution = MessageTools.createKinematicsToolboxOutputStatus(oneDoFJoints); inverseKinematicsSolution.setDestination(-1); gains.setProportionalGains(1200.0); // Gains used for everything. It is as high as possible to reduce the convergence time. gains.setMaxFeedbackAndFeedbackRate(1500.0, Double.POSITIVE_INFINITY); privilegedWeight.set(1.0); privilegedConfigurationGain.set(50.0); privilegedMaxVelocity.set(Double.POSITIVE_INFINITY); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testTree() { Random random = new Random(167L); Robot robot = new Robot("robot"); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); LinkedHashMap<RevoluteJoint, PinJoint> jointMap = new LinkedHashMap<RevoluteJoint, PinJoint>(); RigidBodyBasics elevator = new RigidBody("elevator", worldFrame); double gravity = 0.0; int numberOfJoints = 3; InverseDynamicsCalculatorSCSTest.createRandomTreeRobotAndSetJointPositionsAndVelocities(robot, jointMap, worldFrame, elevator, numberOfJoints, gravity, true, true, random); robot.updateVelocities(); CenterOfMassReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("com", worldFrame, elevator); centerOfMassFrame.update(); CentroidalMomentumCalculator centroidalMomentumMatrix = new CentroidalMomentumCalculator(elevator, centerOfMassFrame); centroidalMomentumMatrix.reset(); Momentum comMomentum = computeCoMMomentum(elevator, centerOfMassFrame, centroidalMomentumMatrix); Point3D comPoint = new Point3D(); Vector3D linearMomentum = new Vector3D(); Vector3D angularMomentum = new Vector3D(); robot.computeCOMMomentum(comPoint, linearMomentum, angularMomentum); EuclidCoreTestTools.assertTuple3DEquals(linearMomentum, comMomentum.getLinearPart(), 1e-12); EuclidCoreTestTools.assertTuple3DEquals(angularMomentum, comMomentum.getAngularPart(), 1e-12); }
LegReferenceFrames(RigidBodyBasics pelvis, RigidBodyBasics elevator, SideDependentList<RigidBodyBasics> feet, SideDependentList<MovingReferenceFrame> soleFrames) { pelvisFrame = pelvis.getBodyFixedFrame(); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", ReferenceFrame.getWorldFrame(), elevator); for (RobotSide robotSide : RobotSide.values) { footReferenceFrames.put(robotSide, feet.get(robotSide).getBodyFixedFrame()); soleReferenceFrames.put(robotSide, soleFrames.get(robotSide)); } }
new RigidBody("rigidBody", sixDoFJoint, momentOfInertia, mass, comOffset); CenterOfMassReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("com", worldFrame, elevator); CentroidalMomentumCalculator centroidalMomentumMatrix = new CentroidalMomentumCalculator(elevator, centerOfMassFrame); elevator.updateFramesRecursively(); centerOfMassFrame.update();
centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", ReferenceFrame.getWorldFrame(), elevator);
int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(joints); CenterOfMassReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", worldFrame, rootBody); SpatialAccelerationCalculator spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame()); spatialAccelerationCalculator.setGravitionalAcceleration(-0.0); joints.get(0).updateFramesRecursively(); centerOfMassFrame.update(); joints.get(0).updateFramesRecursively(); centerOfMassFrame.update();
centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", ReferenceFrame.getWorldFrame(), elevator);
CenterOfMassReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("com", worldFrame, elevator); robot.updateJointTorques_ID_to_SCS(); elevator.updateFramesRecursively(); centerOfMassFrame.update(); robot.updateJointVelocities_SCS_to_ID(); elevator.updateFramesRecursively(); centerOfMassFrame.update();
JointBasics[] jointsArray = new RevoluteJoint[joints.size()]; joints.toArray(jointsArray); ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", world, elevator);
PlanarRobotArm() { elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", ReferenceFrame.getWorldFrame(), elevator); upperArm = createUpperArm(elevator); lowerArm = createLowerArm(upperArm); hand = createHand(lowerArm); scsRobotArm = new SCSRobotFromInverseDynamicsRobotModel("robotArm", elevator.getChildrenJoints().get(0)); scsRobotArm.setGravity(0); scsRobotArm.updateJointPositions_ID_to_SCS(); scsRobotArm.update(); addLinkGraphics(); addForcePoint(); oneDoFJoints = MultiBodySystemTools.createOneDoFJointPath(elevator, hand); elevator.updateFramesRecursively(); }
ReferenceFrame centerOfMassReferenceFrame = new CenterOfMassReferenceFrame("com", elevator.getBodyFixedFrame(), elevator);
RobotArm() { elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", ReferenceFrame.getWorldFrame(), elevator); shoulderDifferentialYaw = createDifferential("shoulderDifferential", elevator, new Vector3D(), Z); RigidBodyBasics shoulderDifferentialRoll = createDifferential("shoulderDifferential", shoulderDifferentialYaw, new Vector3D(), X); RigidBodyBasics upperArm = createUpperArm(shoulderDifferentialRoll); RigidBodyBasics lowerArm = createLowerArm(upperArm); RigidBodyBasics wristDifferentialRoll = createDifferential("wristDifferential", lowerArm, new Vector3D(0.0, 0.0, SHIN_LENGTH), X); //RigidBody wristDifferentialYaw = createDifferential("wristDifferential", wristDifferentialRoll, new Vector3d(), Z); hand = createHand(wristDifferentialRoll); scsRobotArm = new SCSRobotFromInverseDynamicsRobotModel("robotArm", elevator.getChildrenJoints().get(0)); scsRobotArm.setGravity(0); scsRobotArm.updateJointPositions_ID_to_SCS(); scsRobotArm.update(); addLinkGraphics(); addForcePoint(); oneDoFJoints = MultiBodySystemTools.createOneDoFJointPath(elevator, hand); elevator.updateFramesRecursively(); }
centerOfMassFrame = new CenterOfMassReferenceFrame("CoM", ReferenceFrame.getWorldFrame(), fullRobotModel.getElevator()); constrainedCentroidalMomentumMatrixCalculator = new ConstrainedCentroidalMomentumMatrixCalculator(fullRobotModel.getRootJoint(), centerOfMassFrame, momentumSelectionMatrix);
joints.get(0).getPredecessor().updateFramesRecursively(); ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); double controlDT = 0.004;
joints.get(0).getPredecessor().updateFramesRecursively(); ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); double controlDT = 0.004;
centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", worldFrame, elevator);
RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); double controlDT = 0.004;
RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); double controlDT = 0.004;
joints.get(0).getPredecessor().updateFramesRecursively(); ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); double controlDT = 0.004;
joints.get(0).getPredecessor().updateFramesRecursively(); ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); double controlDT = 0.004;