public double getHandleAngle() { return handlePinJoint.getQYoVariable().getDoubleValue(); }
public double getElbowAngle() { return elbowJoint.getQYoVariable().getDoubleValue(); }
public double getHingeYaw() { return doorHingePinJoint.getQYoVariable().getDoubleValue(); }
@Override public void notifyOfVariableChange(YoVariable<?> v) { double rangeOfMotion = 2 * Math.PI * totalNumberOfPossibleTurns; steeringWheelAngleAsAbsolutePercentageOfRangeOfMotion.set((Math.abs(steeringWheelPinJoint.getQYoVariable().getDoubleValue()) / (0.5 * rangeOfMotion)) * 100); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { valveClosePercentage.set(valvePinJoint.getQYoVariable().getDoubleValue()/(2*Math.PI)*100/valveNumberOfPossibleTurns); } });
public double getPosition() { return getPinJoint().getQYoVariable().getDoubleValue(); }
PinJointRobotSensor(PinJoint joint) { q = joint.getQYoVariable(); qd = joint.getQDYoVariable(); qdd = joint.getQDDYoVariable(); tau_actual = joint.getTauYoVariable(); }
@Override public void read(long currentClockTime) { q_j1.set(doublePendulum.getJ1().getQYoVariable().getDoubleValue()); qd_j1.set(doublePendulum.getJ1().getQDYoVariable().getDoubleValue()); q_j2.set(doublePendulum.getJ2().getQYoVariable().getDoubleValue()); qd_j2.set(doublePendulum.getJ2().getQDYoVariable().getDoubleValue()); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newPose = new RigidBodyTransform(); pinJointTransform.setRotationYawAndZeroTranslation(steeringWheelPinJoint.getQYoVariable().getDoubleValue()); newPose.set(originalSteeringWheelPose); newPose.multiply(pinJointTransform); steeringWheelFrame.setPoseAndUpdate(newPose); super.updateAllGroundContactPointVelocities(); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newValvePose = new RigidBodyTransform(); pinJointTransform.setRotationRollAndZeroTranslation(valvePinJoint.getQYoVariable().getDoubleValue()); newValvePose.set(originalValvePose); newValvePose.multiply(pinJointTransform); valveFrame.setPoseAndUpdate(newValvePose); super.updateAllGroundContactPointVelocities(); }
DataBufferEntry position = dataBuffer.getEntry(joint.getQYoVariable()); DataBufferEntry speed = dataBuffer.getEntry(joint.getQDYoVariable()); DataBufferEntry torque = dataBuffer.getEntry(joint.getTauYoVariable());
private void writeJointDataToWorkbook(WritableWorkbook workbook) { WritableSheet jointDataSheet = workbook.createSheet("Joint Data", workbook.getNumberOfSheets()); int column = 0; writeJointDataColumn(jointDataSheet, column++, dataBuffer.getEntry("t"), true); for (PinJoint joint : pinJoints) { DataBufferEntry position = dataBuffer.getEntry(joint.getQYoVariable()); DataBufferEntry speed = dataBuffer.getEntry(joint.getQDYoVariable()); DataBufferEntry torque = dataBuffer.getEntry(joint.getTauYoVariable()); writeJointDataColumn(jointDataSheet, column++, position, false); writeJointDataColumn(jointDataSheet, column++, speed, false); writeJointDataColumn(jointDataSheet, column++, speed, true); writeJointDataColumn(jointDataSheet, column++, torque, false); writeJointDataColumn(jointDataSheet, column++, torque, true); writeMechanicalPowerJointDataColumn(jointDataSheet, column++, speed, torque, joint.getName()); } }
l_leg_hpz.setQ(l_hip_yaw.getQYoVariable().getDoubleValue()); r_leg_hpz.setQ(r_hip_yaw.getQYoVariable().getDoubleValue()); RigidBodyBasics leftHipDifferentialBody = copyLinkAsRigidBody(l_hip_differential, l_leg_hpz, "l_hip_differential"); RigidBodyBasics rightHipDifferentialBody = copyLinkAsRigidBody(r_hip_differential, r_leg_hpz, "r_hip_differential"); l_leg_hpx.setQ(l_hip_roll.getQYoVariable().getDoubleValue()); r_leg_hpx.setQ(r_hip_roll.getQYoVariable().getDoubleValue()); RigidBodyBasics leftHipDifferentialBody2 = copyLinkAsRigidBody(l_hip_differential2, l_leg_hpx, "l_hip_differential"); RigidBodyBasics rightHipDifferentialBody2 = copyLinkAsRigidBody(r_hip_differential2, r_leg_hpx, "r_hip_differential"); l_leg_hpy.setQ(l_hip_pitch.getQYoVariable().getDoubleValue()); r_leg_hpy.setQ(r_hip_pitch.getQYoVariable().getDoubleValue()); RigidBodyBasics leftThighBody = copyLinkAsRigidBody(leftThigh, l_leg_hpy, "l_thigh"); RigidBodyBasics rightThighBody = copyLinkAsRigidBody(rightThigh, r_leg_hpy, "r_thigh"); l_leg_kny.setQ(l_knee_pitch.getQYoVariable().getDoubleValue()); r_leg_kny.setQ(r_knee_pitch.getQYoVariable().getDoubleValue()); RigidBodyBasics leftShinBody = copyLinkAsRigidBody(l_shin, l_leg_kny, "l_shin"); RigidBodyBasics rightShinBody = copyLinkAsRigidBody(r_shin, r_leg_kny, "r_shin"); l_leg_aky.setQ(l_ankle_pitch.getQYoVariable().getDoubleValue()); r_leg_aky.setQ(r_ankle_pitch.getQYoVariable().getDoubleValue()); RigidBodyBasics leftAnkleDifferentialBody = copyLinkAsRigidBody(l_ankle_differential, l_leg_aky, "l_ankle_differential"); RigidBodyBasics rightAnkleDifferentialBody = copyLinkAsRigidBody(r_ankle_differential, r_leg_aky, "r_ankle_differential"); l_leg_akx.setQ(l_ankle_roll.getQYoVariable().getDoubleValue()); r_leg_akx.setQ(r_ankle_roll.getQYoVariable().getDoubleValue()); RigidBodyBasics leftFootBody = copyLinkAsRigidBody(l_foot, l_leg_akx, "l_foot"); RigidBodyBasics rightFootBody = copyLinkAsRigidBody(r_foot, r_leg_akx, "r_foot");
lidarJoint.getQYoVariable().add(desiredLidarVelocity.getDoubleValue() * dt);
valvePinJoint.getQYoVariable().addVariableChangedListener(new VariableChangedListener()
assertFalse(Double.isNaN(joint1.getQYoVariable().getDoubleValue())); assertEquals(joint1.getQYoVariable().getDoubleValue(), joint1B.getQYoVariable().getDoubleValue(), epsilon); assertEquals(joint1.getQDYoVariable().getDoubleValue(), joint1B.getQDYoVariable().getDoubleValue(), epsilon); assertEquals(joint1.getQDDYoVariable().getDoubleValue(), joint1B.getQDDYoVariable().getDoubleValue(), epsilon); assertEquals(joint2.getQYoVariable().getDoubleValue(), joint2B.getQYoVariable().getDoubleValue(), epsilon); assertEquals(joint2.getQDYoVariable().getDoubleValue(), joint2B.getQDYoVariable().getDoubleValue(), epsilon); assertEquals(joint2.getQDDYoVariable().getDoubleValue(), joint2B.getQDDYoVariable().getDoubleValue(), epsilon);
steeringWheelPinJoint.getQYoVariable().addVariableChangedListener(new VariableChangedListener()
pin2.setQ(-pin1.getQYoVariable().getDoubleValue());