/** * Only place where the SCS robot should be modified */ public void writeDesiredJointAngles() { for (int i = 0; i < allFingerJoints.size(); i++) { OneDegreeOfFreedomJoint fingerJoint = allFingerJoints.get(i); fingerJoint.setqDesired(desiredAngles.get(fingerJoint).getDoubleValue()); } }
/** * Only place where the SCS robot should be modified */ public void writeDesiredJointAngles() { for (int i = 0; i < allFingerJoints.size(); i++) { OneDegreeOfFreedomJoint fingerJoint = allFingerJoints.get(i); fingerJoint.setqDesired(desiredAngles.get(fingerJoint).getDoubleValue()); } }
shoulderYaw.setKp(200.0); shoulderYaw.setKd(20.0); shoulderYaw.setqDesired(qDesireds.get(shoulderYaw)); OneDegreeOfFreedomJoint shoulderRoll = robot.getOneDegreeOfFreedomJoint(jointMap.getArmJointName(robotSide, ArmJointName.SHOULDER_ROLL)); shoulderRoll.setKp(200.0); shoulderRoll.setKd(20.0); shoulderRoll.setqDesired(qDesireds.get(shoulderRoll)); OneDegreeOfFreedomJoint elbowPitch = robot.getOneDegreeOfFreedomJoint(jointMap.getArmJointName(robotSide, ArmJointName.ELBOW_PITCH)); elbowPitch.setKp(200.0); elbowPitch.setKd(20.0); elbowPitch.setqDesired(qDesireds.get(elbowPitch)); OneDegreeOfFreedomJoint elbowRoll = robot.getOneDegreeOfFreedomJoint(jointMap.getArmJointName(robotSide, ArmJointName.ELBOW_ROLL)); elbowRoll.setKp(200.0); elbowRoll.setKd(20.0); elbowRoll.setqDesired(qDesireds.get(elbowRoll)); OneDegreeOfFreedomJoint wristPitch = robot.getOneDegreeOfFreedomJoint(jointMap.getArmJointName(robotSide, ArmJointName.FIRST_WRIST_PITCH)); wristPitch.setKp(20.0); wristPitch.setKd(2.0); wristPitch.setqDesired(qDesireds.get(wristPitch)); OneDegreeOfFreedomJoint wristRoll = robot.getOneDegreeOfFreedomJoint(jointMap.getArmJointName(robotSide, ArmJointName.WRIST_ROLL)); wristRoll.setKp(20.0); wristRoll.setKd(2.0); wristRoll.setqDesired(qDesireds.get(wristRoll)); hipPitch.setqDesired(qDesireds.get(hipPitch)); OneDegreeOfFreedomJoint hipRoll = robot.getOneDegreeOfFreedomJoint(jointMap.getLegJointName(robotSide, LegJointName.HIP_ROLL)); hipRoll.setKp(500.0); hipRoll.setKd(50.0); hipRoll.setqDesired(qDesireds.get(hipRoll));
@Override public void writeAfterController(long timestamp) { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); double tau = revoluteJoint.getTau(); DoubleYoVariable rawJointTorque = rawJointTorques.get(revoluteJoint); DelayedDoubleYoVariable delayedJointTorque = delayedJointTorques.get(revoluteJoint); if (rawJointTorque != null) { rawJointTorque.set(tau); delayedJointTorque.update(); tau = delayedJointTorque.getDoubleValue(); } pinJoint.setTau(tau); pinJoint.setKp(revoluteJoint.getKp()); pinJoint.setKd(revoluteJoint.getKd()); pinJoint.setqDesired(revoluteJoint.getqDesired()); pinJoint.setQdDesired(revoluteJoint.getQdDesired()); } for (int i = 0; i < rawOutputWriters.size(); i++) { rawOutputWriters.get(i).write(); } }
data.simulatedJoint.setqDesired(data.jointData.getDesiredPosition());
protected void write() { for (int i = 0; i < revoluteJoints.size(); i++) { OneDegreeOfFreedomJoint pinJoint = revoluteJoints.first(i); JointDesiredOutputReadOnly data = revoluteJoints.second(i); if(data.hasDesiredTorque()) { pinJoint.setTau(data.getDesiredTorque()); } if (data.hasStiffness()) { pinJoint.setKp(data.getStiffness()); } if (data.hasDamping()) { pinJoint.setKd(data.getDamping()); } if (data.hasDesiredPosition()) { pinJoint.setqDesired(data.getDesiredPosition()); } if (data.hasDesiredVelocity()) { pinJoint.setQdDesired(data.getDesiredVelocity()); } } }