/** ID Robot --> SCS Robot Copy the torques from the IDRobot to the SCSRobot. */
 public void updateTorquesSCSrobot() // Remember! the body joint is NOT actuated
     {
   for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
     OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
     scsJoint.setTau(idJoint.getTau());
   }
 }
  /** SCS Robot --> ID Robot Send positions and velocities. */
  public void updatePositionsIDrobot() {
    // Body info
    bodyJointID.setPosition(
        bodyJointSCS.getQ_t1().getDoubleValue(), 0.0, bodyJointSCS.getQ_t2().getDoubleValue());
    bodyJointID.setRotation(0.0, bodyJointSCS.getQ_rot().getDoubleValue(), 0.0);

    double[] velocityArray = new double[6];
    velocityArray[0] = 0.0; // yaw
    velocityArray[1] = bodyJointSCS.getQd_rot().getDoubleValue(); // pitch
    velocityArray[2] = 0.0; // roll
    velocityArray[3] = bodyJointSCS.getQd_t1().getDoubleValue(); // x
    velocityArray[4] = 0.0; // y
    velocityArray[5] = bodyJointSCS.getQd_t2().getDoubleValue(); // z
    DenseMatrix64F velocities = new DenseMatrix64F(6, 1, true, velocityArray);
    bodyJointID.setVelocity(velocities, 0);

    // Leg  and foot info (hip, knee and ankle)
    for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
      OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
      idJoint.setQ(scsJoint.getQYoVariable().getDoubleValue());
      idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue());
    }

    elevator.updateFramesRecursively();

    // Get the body coordinates
    bodyPosition = new FramePoint();
    bodyPosition.setToZero(bodyJointID.getFrameAfterJoint());
    bodyPosition.changeFrame(worldFrame);
  }
コード例 #3
0
  public void setFingerJointsProvider(
      SideDependentList<List<OneDegreeOfFreedomJoint>> allFingerJoints) {
    fingerJointMap = new HashMap<String, OneDegreeOfFreedomJoint>();

    for (RobotSide robotSide : RobotSide.values) {
      for (OneDegreeOfFreedomJoint joint : allFingerJoints.get(robotSide)) {
        fingerJointMap.put(joint.getName(), joint);
      }
    }
  }
  @Override
  public void write() {
    for (int i = 0; i < revoluteJoints.size(); i++) {
      ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i);
      OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
      OneDoFJoint revoluteJoint = jointPair.getRight();

      pinJoint.setTau(revoluteJoint.getTau());
    }
  }
  public void updateRobotConfigurationBasedOnFullRobotModel() {
    for (int i = 0; i < revoluteJoints.size(); i++) {
      ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i);
      OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
      OneDoFJoint revoluteJoint = jointPair.getRight();

      pinJoint.setQ(revoluteJoint.getQ());
    }

    FloatingJoint floatingJoint = rootJointPair.getLeft();
    FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight();

    RigidBodyTransform transform = sixDoFJoint.getJointTransform3D();
    floatingJoint.setRotationAndTranslation(transform);
  }