/** 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); }