public void setPosition(Tuple3DBasics position) { qrCodeJoint.setPosition(position); }
public void setPosition(double x, double y, double z) { qrCodeJoint.setPosition(x, y, z); }
public void setPosition(Tuple3DBasics position) { qrCodeJoint.setPosition(position); }
public void setPosition(double x, double y, double z) { qrCodeJoint.setPosition(x, y, z); }
public void setPosition(Tuple3DBasics position) { getFloatingJoint().setPosition(position); }
public void setPosition(double x, double y, double z) { getFloatingJoint().setPosition(x, y, z); }
public void setPosition(double[] position) { getFloatingJoint().setPosition(position[0], position[1], position[2]); }
private void setPosition(FloatingJoint cubeJoint, double x, double y, double z) { cubeJoint.setPosition(x, y, z); cubeJoint.getRobot().update(); } }
private void setRandomPosition(FloatingJoint floatingJoint, SixDoFJoint sixDoFJoint) { Point3D rootPosition = new Point3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); double yaw = random.nextDouble(); double pitch = random.nextDouble(); double roll = random.nextDouble(); floatingJoint.setPosition(rootPosition); floatingJoint.setYawPitchRoll(yaw, pitch, roll); sixDoFJoint.setJointPosition(rootPosition); sixDoFJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll); }
public void get(FloatingJoint joint) { rotationMatrix.set(rotation); joint.setRotation(rotationMatrix); joint.setPosition(translation); twist.getAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); twist.getLinearPart(tempVector); joint.setVelocity(tempVector); }
@Override public void update() { jointState.getRotation(rotationMatrix); jointState.getTranslation(tempVector); joint.setRotation(rotationMatrix); joint.setPosition(tempVector); jointState.getTwistAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); jointState.getTwistLinearPart(tempVector); joint.setVelocity(tempVector); } }
Vector3D velocityOne = new Vector3D(12.4, 15.9, 0.2); rootJointOne.setPosition(positionOne); rootJointOne.setVelocity(velocityOne); Vector3D velocityTwo = new Vector3D(velocityOne); rootJointTwo.setPosition(positionTwo); rootJointTwo.setVelocity(velocityTwo);
@Override public void update() { jointState.getRotation(rotationMatrix); jointState.getTranslation(tempVector); joint.setRotation(rotationMatrix); joint.setPosition(tempVector); jointState.getTwistAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); jointState.getTwistLinearPart(tempVector); joint.setVelocity(tempVector); } }
public void testBoxCloseButNoCollisions() { ScsCollisionDetector collisionDetector = createCollisionDetector(); FloatingJoint cubeA = cube(collisionDetector, "A", 10, 0.5, 1.0, 1.5); FloatingJoint cubeB = cube(collisionDetector, "B", 10, 0.75, 1.2, 1.7); double a[] = new double[] { 0.5 + 0.75, 1.0 + 1.2, 1.5 + 1.7 }; // add a bit of separation to ensure they don't collide double tau = 0.001; // should just barely not intersect for (int i = 0; i < 3; i++) { double Tx, Ty, Tz; Tx = Ty = Tz = 0.0; if (i == 0) Tx = a[i] / 2.0 + tau; if (i == 1) Ty = a[i] / 2.0 + tau; if (i == 2) Tz = a[i] / 2.0 + tau; cubeA.setPosition(Tx, Ty, Tz); cubeB.setPosition(-Tx, -Ty, -Tz); cubeA.getRobot().update(); cubeB.getRobot().update(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); cubeA.getTransformToWorld(transformToWorld); CollisionDetectionResult result = new CollisionDetectionResult(); collisionDetector.performCollisionDetection(result); assertEquals(0, result.getNumberOfCollisions()); } }
Tz = a[i] / 2.0 + tau; cubeA.setPosition(Tx, Ty, Tz); cubeB.setPosition(-Tx, -Ty, -Tz);
@Override public void doControl() { refFrame.update(); sixDofPelvisJoint.setJointConfiguration(robotPose); pelvisPoseHistoryCorrection.doControl(Conversions.secondsToNanoseconds(scs.getTime())); floatingJoint.setQuaternion(sixDofPelvisJoint.getJointPose().getOrientation()); floatingJoint.setPosition(sixDofPelvisJoint.getJointPose().getPosition()); } }
FloatingJoint cubeC = cube(collisionDetector, "C", 10, offset, 1, 1, 1, 2, 2); cubeA.setPosition(0, 0, 0.5); cubeB.setPosition(0, 0, 0.5);
public void collisionMask_hit() { ScsCollisionDetector collisionDetector = createCollisionDetector(); // all 3 shapes will overlap. the mask determines what intersects FloatingJoint cubeA = cube(collisionDetector, "A", 10, null, 0.5, 1, 1.5, 0x01, 0x02); FloatingJoint cubeB = cube(collisionDetector, "A", 10, null, 0.75, 1.2, 1.7, 0x02, 0x01); FloatingJoint cubeC = cube(collisionDetector, "A", 10, null, 10, 10, 10, 0x04, 0x04); // just do an offset so that not everything is at the origin cubeB.setPosition(0.4, 0, 0); cubeA.update(); cubeB.update(); cubeC.update(); CollisionDetectionResult result = new CollisionDetectionResult(); collisionDetector.performCollisionDetection(result); assertEquals(1, result.getNumberOfCollisions()); }
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)); } }
@Test// timeout=300000 public void testCalculateAngularMomentum() { double epsilon = 1e-7; Robot robot1 = new Robot("r1"); FloatingJoint floatingJoint1 = new FloatingJoint("joint1", new Vector3D(),robot1); robot1.addRootJoint(floatingJoint1); Link onlyLink=new Link("SphericalLink"); onlyLink.setComOffset(new Vector3D(0.0, 0.0, 0.0)); onlyLink.setMass(1.0); onlyLink.setMomentOfInertia(1.0, 1.0, 1.0); floatingJoint1.setLink(onlyLink); floatingJoint1.setPosition(new Point3D(1.0,1.0,1.0)); floatingJoint1.setAngularVelocityInBody(new Vector3D(1.0,0.0,0.0)); floatingJoint1.setVelocity(-1.0, 0.0, 0.0); Vector3D angularMomentumReturned = new Vector3D(); robot1.computeAngularMomentum(angularMomentumReturned); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, 0.0), angularMomentumReturned, epsilon); robot1.update(); robot1.computeAngularMomentum(angularMomentumReturned); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, 0.0), angularMomentumReturned, epsilon); robot1.updateVelocities(); robot1.computeAngularMomentum(angularMomentumReturned); //System.out.printf("Momentum <%+3.4f,%+3.4f,%+3.4f>", angularMomentumReturned.x,angularMomentumReturned.y,angularMomentumReturned.z); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(1.0, -1.0, 1.0), angularMomentumReturned, epsilon); floatingJoint1.setPosition(new Vector3D()); }