public ConstrainedCenterOfMassJacobianCalculator(FloatingInverseDynamicsJoint rootJoint) { this.dynamicallyConsistentNullspaceCalculator = new OriginalDynamicallyConsistentNullspaceCalculator(rootJoint, true); this.centerOfMassJacobian = new CenterOfMassJacobian(rootJoint.getSuccessor()); }
public void compute() { dynamicallyConsistentNullspaceCalculator.compute(); centerOfMassJacobian.compute(); DenseMatrix64F centerOfMassJacobianMatrix = centerOfMassJacobian.getMatrix(); DenseMatrix64F sNsBar = dynamicallyConsistentNullspaceCalculator.getSNsBar(); constrainedCenterOfMassJacobian.reshape(centerOfMassJacobianMatrix.getNumRows(), sNsBar.getNumCols()); CommonOps.mult(centerOfMassJacobianMatrix, sNsBar, constrainedCenterOfMassJacobian); }
public void getCenterOfMassVelocity(FrameVector centerOfMassVelocityToPack) { ScrewTools.getJointVelocitiesMatrix(joints, tempJointVelocitiesMatrix); getCenterOfMassVelocity(centerOfMassVelocityToPack, tempJointVelocitiesMatrix); }
public void compute() { int column = 0; for (int i = 0; i < rigidBodyList.size(); i++) { comScaledByMassMapIsUpdated.get(rigidBodyList.get(i)).setValue(false); } for (InverseDynamicsJoint joint : joints) { RigidBody childBody = joint.getSuccessor(); FramePoint comPositionScaledByMass = getCoMScaledByMass(childBody); double subTreeMass = getSubTreeMass(childBody); GeometricJacobian motionSubspace = joint.getMotionSubspace(); final List<Twist> allTwists = motionSubspace.getAllUnitTwists(); for(int i = 0; i < allTwists.size(); i++) { tempUnitTwist.set(allTwists.get(i)); tempUnitTwist.changeFrame(rootFrame); setColumn(tempUnitTwist, comPositionScaledByMass, subTreeMass, column); column++; } } CommonOps.scale(inverseTotalMass, jacobianMatrix); }
private void updateGroundTruth() { FramePoint com = new FramePoint(); perfectCenterOfMassCalculator.compute(); perfectCenterOfMassCalculator.getCenterOfMass(com); perfectCoM.set(com); FrameVector comd = new FrameVector(); perfectCenterOfMassJacobian.compute(); perfectCenterOfMassJacobian.getCenterOfMassVelocity(comd); comd.changeFrame(ReferenceFrame.getWorldFrame()); perfectCoMd.set(comd); FrameVector comdd = new FrameVector(); perfectCenterOfMassAccelerationCalculator.getCoMAcceleration(comdd); comdd.changeFrame(ReferenceFrame.getWorldFrame()); perfectCoMdd.set(comdd); }
public void update() { referenceFrames.updateFrames(); twistCalculator.compute(); centerOfMassJacobian.compute(); if (referenceFramesVisualizer != null) referenceFramesVisualizer.update(); computeCop(); computeCapturePoint(); updateBipedSupportPolygons(); readWristSensorData(); computeAngularMomentum(); robotJacobianHolder.compute(); for (int i = 0; i < updatables.size(); i++) updatables.get(i).update(yoTime.getDoubleValue()); for (RobotSide robotSide : RobotSide.values) footSwitches.get(robotSide).updateCoP(); }
public DenseMatrix64F getCenterOfMassJacobian() { return centerOfMassJacobian.getMatrix(); } }
private double getSubTreeMass(RigidBody rigidBody) { if (!subTreeMassMap.containsKey(rigidBody)) subTreeMassMap.put(rigidBody, new MutableDouble(-1.0)); MutableDouble subTreeMass = subTreeMassMap.get(rigidBody); if (subTreeMass.doubleValue() > 0.0) { return subTreeMass.doubleValue(); } else { double curSubTreeMass = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); List<InverseDynamicsJoint> childrenJoints = rigidBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { double childSubTreeMass = getSubTreeMass(childrenJoints.get(i).getSuccessor()); curSubTreeMass = curSubTreeMass + childSubTreeMass; } subTreeMass.setValue(curSubTreeMass); return curSubTreeMass; } }
for (int i = 0; i < childrenJoints.size(); i++) curChildCoMScaledByMass.add(getCoMScaledByMass(childrenJoints.get(i).getSuccessor()));
private void computeRootJointLinearVelocity(FrameVector centerOfMassVelocityWorld, FrameVector rootJointVelocityToPack, FrameVector rootJointAngularVelocity, FloatingInverseDynamicsJoint rootJoint) { tempCenterOfMassVelocityWorld.setIncludingFrame(centerOfMassVelocityWorld); ReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint(); // \dot{r}^{root} centerOfMassJacobianBody.compute(); tempComVelocityBody.setToZero(rootJointFrame); centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody); tempComVelocityBody.changeFrame(rootJointFrame); // \tilde{\omega} r^{root} tempComBody.setToZero(rootJointFrame); centerOfMassCalculator.getCenterOfMass(tempComBody); tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame); tempCrossPart.cross(rootJointAngularVelocity, tempComBody); // v_{r/p}= \tilde{\omega} r^{root} + \dot{r}^{root} tempCenterOfMassVelocityOffset.setToZero(rootJointFrame); tempCenterOfMassVelocityOffset.add(tempCrossPart, tempComVelocityBody); // v_{root}^{p,w} = R_{w}^{root} \dot{r} - v_{r/p} tempCenterOfMassVelocityWorld.changeFrame(rootJointFrame); rootJointVelocityToPack.setIncludingFrame(tempCenterOfMassVelocityWorld); rootJointVelocityToPack.sub(tempCenterOfMassVelocityOffset); }
yoCenterOfMassPosition.set(centerOfMassPosition); centerOfMassJacobianWorld.compute(); centerOfMassVelocityUsingPelvisIMUAndKinematics.setToZero(ReferenceFrame.getWorldFrame()); centerOfMassJacobianWorld.getCenterOfMassVelocity(centerOfMassVelocityUsingPelvisIMUAndKinematics); centerOfMassVelocityUsingPelvisIMUAndKinematics.changeFrame(ReferenceFrame.getWorldFrame()); yoCenterOfMassVelocityUsingPelvisAndKinematics.set(centerOfMassVelocityUsingPelvisIMUAndKinematics);
public PositionStateRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionOutputPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityOutputPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionOutputPort = centerOfMassPositionOutputPort; this.centerOfMassVelocityOutputPort = centerOfMassVelocityOutputPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); }
private void computeCapturePoint() { centerOfMassPosition.setToZero(centerOfMassFrame); if (centerOfMassDataHolder != null) { centerOfMassDataHolder.getCenterOfMassVelocity(centerOfMassVelocity); } else { centerOfMassJacobian.getCenterOfMassVelocity(centerOfMassVelocity); } centerOfMassPosition.changeFrame(worldFrame); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition); centerOfMassVelocity2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassVelocity); CapturePointCalculator.computeCapturePoint(capturePoint2d, centerOfMassPosition2d, centerOfMassVelocity2d, omega0.getDoubleValue()); capturePoint2d.changeFrame(yoCapturePoint.getReferenceFrame()); yoCapturePoint.setXY(capturePoint2d); }
centerOfMassJacobianBody.compute(); tempComVelocityBody.setToZero(rootJointFrame); centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody); tempComVelocityBody.changeFrame(rootJointFrame);
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); }
centerOfMassJacobian.getCenterOfMassVelocity(comVelocity); comPosition.changeFrame(worldFrame); comVelocity.changeFrame(worldFrame);
public OrientationAndPositionFullRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityPort, ControlFlowOutputPort<FrameVector> centerOfMassAccelerationPort, ControlFlowOutputPort<FrameOrientation> orientationPort, ControlFlowOutputPort<FrameVector> angularVelocityPort, ControlFlowOutputPort<FrameVector> angularAccelerationPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionPort = centerOfMassPositionPort; this.centerOfMassVelocityPort = centerOfMassVelocityPort; this.centerOfMassAccelerationPort = centerOfMassAccelerationPort; this.orientationPort = orientationPort; this.angularVelocityPort = angularVelocityPort; this.angularAccelerationPort = angularAccelerationPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); // TODO: Should pass the input port for the spatial acceleration calculator here too... this.centerOfMassAccelerationCalculator = new CenterOfMassAccelerationCalculator(rootJoint.getSuccessor(), ScrewTools.computeSupportAndSubtreeSuccessors(elevator), inverseDynamicsStructure.getSpatialAccelerationCalculator()); }
this.omega0.set(omega0); this.centerOfMassJacobian = new CenterOfMassJacobian(fullRobotModel.getElevator()); if (yoGraphicsListRegistry != null)
this.centerOfMassJacobianWorld = new CenterOfMassJacobian(elevator);