@Override public void get(OneDegreeOfFreedomJoint joint) { if(!Double.isNaN(q)) { joint.setQ(q); } if(!Double.isNaN(qd)) { joint.setQd(qd); } }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } }
@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.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } } }
@Override public void update() { double q = jointState.getQ(); double qd = jointState.getQd(); if (!Double.isNaN(q)) { joint.setQ(q); } if (!Double.isNaN(qd)) { joint.setQd(qd); } } }
@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.setQ(revoluteJoint.getqDesired()); pinJoint.setQd(revoluteJoint.getQdDesired()); pinJoint.setQdd(revoluteJoint.getQddDesired()); } } }
@Override public void update() { double q = jointState.getQ(); double qd = jointState.getQd(); if (!Double.isNaN(q)) { joint.setQ(q); } if (!Double.isNaN(qd)) { joint.setQd(qd); } } }
public void updateRobotConfigurationBasedOnJointDesiredOutputPositions() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); double q = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredPosition(); pinJoint.setQ(q); double qd = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredVelocity(); pinJoint.setQd(qd); double qdd = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredAcceleration(); pinJoint.setQdd(qdd); } }
public static void setRandomVelocity(Random random, Robot robot) { FloatingJoint rootJoint = (FloatingJoint) robot.getRootJoints().get(0); rootJoint.setVelocity(EuclidCoreRandomTools.nextVector3D(random)); rootJoint.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) joint.setQd(EuclidCoreRandomTools.nextDouble(random)); }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); JointDesiredOutputReadOnly jointOutput = jointDesiredOutputListReadOnly.getJointDesiredOutput(revoluteJoint); if (jointOutput.hasDesiredPosition()) pinJoint.setQ(jointOutput.getDesiredPosition()); if (jointOutput.hasDesiredVelocity()) pinJoint.setQd(jointOutput.getDesiredVelocity()); } } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); } }
public void setRobotStateToMatchOtherRobot(FloatingRootJointRobot otherRobot) { otherRobot.update(); FloatingJoint floatingJoint = robot.getRootJoint(); FloatingJoint otherFloatingJoint = otherRobot.getRootJoint(); Tuple3DBasics position = new Point3D(); Tuple3DBasics velocity = new Vector3D(); otherFloatingJoint.getPositionAndVelocity(position, velocity); floatingJoint.setPositionAndVelocity(position, velocity); Quaternion rotation = new Quaternion(); otherFloatingJoint.getQuaternion(rotation); floatingJoint.setQuaternion(rotation); Vector3D angularVelocityInBody = otherFloatingJoint.getAngularVelocityInBody(); floatingJoint.setAngularVelocityInBody(angularVelocityInBody); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); oneDegreeOfFreedomJoint.setQ(otherOneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue()); oneDegreeOfFreedomJoint.setQd(otherOneDegreeOfFreedomJoint.getQDYoVariable().getDoubleValue()); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair : oneDoFJointPairList) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } if (rootJointPair != null) { FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingJointBasics sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = new RigidBodyTransform(); sixDoFJoint.getJointConfiguration(transform); floatingJoint.setRotationAndTranslation(transform); } } }
public void setRobotStateToMatchFullRobotModel() { FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); FloatingJoint floatingJoint = robot.getRootJoint(); fullRobotModel.updateFrames(); setRobotRootJointPositionAndOrientationToMatchFullRobotModel(sixDoFJoint, floatingJoint); setRobotRootJointVelocityAndAngularVelocityToMatchFullRobotModel(sixDoFJoint, floatingJoint); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); oneDegreeOfFreedomJoint.setQ(oneDoFJoint.getQ()); oneDegreeOfFreedomJoint.setQd(oneDoFJoint.getQd()); } robot.update(); }
pinJoint.setQd(jointVelocity);
public void updateJointVelocities_ID_to_SCS() { if (scsFloatingJoint != null) { ReferenceFrame rootBodyFrame = idFloatingJoint.getFrameAfterJoint(); rootJointTwist.setIncludingFrame(idFloatingJoint.getJointTwist()); linearVelocity.setIncludingFrame(rootJointTwist.getLinearPart()); angularVelocity.setIncludingFrame(rootJointTwist.getAngularPart()); linearVelocity.changeFrame(ReferenceFrame.getWorldFrame()); angularVelocity.changeFrame(rootBodyFrame); scsFloatingJoint.setVelocity(linearVelocity); scsFloatingJoint.setAngularVelocityInBody(angularVelocity); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); scsJoint.setQd(idJoint.getQd()); } }
oneDoFJointsB[jointIndex].setQd(oneDoFJointsA[jointIndex].getQD()); oneDoFJointsB[jointIndex].setQdd(oneDoFJointsA[jointIndex].getQDD());
public void setRobotStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity) { FloatingJoint rootJoint = robot.getRootJoint(); rootJoint.setVelocity(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setAngularVelocityInBody(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setPosition(RandomGeometry.nextVector3D(random)); double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0); double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0); double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0); rootJoint.setYawPitchRoll(yaw, pitch, roll); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { double lowerLimit = oneDegreeOfFreedomJoint.getJointLowerLimit(); double upperLimit = oneDegreeOfFreedomJoint.getJointUpperLimit(); double delta = upperLimit - lowerLimit; lowerLimit = lowerLimit + 0.05 * delta; upperLimit = upperLimit - 0.05 * delta; oneDegreeOfFreedomJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); oneDegreeOfFreedomJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity)); } }
public void update(long timestamp) { fullRobot.updateFrames(); if(rootJoint != null) { RigidBodyTransform rootTransform = rootJoint.getJointTransform3D(); rootTransform.get(tempOrientation, tempPosition); robot.setOrientation(tempOrientation); robot.setPositionInWorld(tempPosition); } for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setTau(revoluteJoint.getTau()); } robot.setTime(robot.getTime() + updateDT); if (scs != null) { scs.tickAndUpdate(); } }
@Override public void update(long timestamp) { fullRobot.updateFrames(); latestTimestamp = timestamp; if(rootJoint != null) { robot.setOrientation(rootJoint.getJointPose().getOrientation()); robot.setPositionInWorld(rootJoint.getJointPose().getPosition()); } for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setTau(revoluteJoint.getTau()); } robot.setTime(robot.getTime() + updateDT); if (scs != null) { scs.tickAndUpdate(); } }