public static System.Numerics.Vector3[] SpherePointCloud(float radius, System.Numerics.Vector3 center, int pointsNum) { var data = new System.Numerics.Vector3[pointsNum]; double x, yPos, y, zPos, z; for (int i = 0; i < pointsNum; i++) { x = radius * (2 * RandomThreadStatic.NextDouble() - 1); yPos = Math.Sqrt(radius * radius - x * x); y = yPos * (2 * RandomThreadStatic.NextDouble() - 1); zPos = Math.Sqrt(yPos * yPos - y * y); z = RandomThreadStatic.Next(0, 2) == 0 ? -zPos : zPos; data[i] = new System.Numerics.Vector3((float)x, (float)y, (float)z) + center; } return(data); }
private BezierCurve Repro(BezierCurve bezierCurve) { int pointIndex = RandomThreadStatic.Next(1, _bezierControlPointsCount + 1); Vector3 direction = RandomThreadStatic.NextPointCube(1); var bezierCurveRepro = new BezierCurve(bezierCurve.Points); var bezierDirectionRepro = Vector3.Normalize(direction) * 1f * (float)RandomThreadStatic.NextDouble(); bezierCurveRepro.Points[pointIndex] += bezierDirectionRepro; if (_manipulator.ApproxWithinReach(bezierCurveRepro.Points[pointIndex] + bezierDirectionRepro)) { bezierCurveRepro.Points[pointIndex] += bezierDirectionRepro; } else { bezierCurveRepro.Points[pointIndex] -= bezierDirectionRepro; } return(bezierCurveRepro); }
public override InverseKinematicsResult Execute(Manipulator manipulator, Vector3 goal, int joint = -1) // TODO: refactor { // use gripper if default joint if (joint == -1) { joint = manipulator.Joints.Length - 1; } VectorFloat configuration = manipulator.q, dq = VectorFloat.Build.Dense(manipulator.Joints.Length); var errorPos = goal - manipulator.Joints[joint].Position; var error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 }); float distance = errorPos.Length(); float maxStepSize = _maxStepSize, scale = 1; int iterations = 0; while (distance > _threshold && iterations < _maxIterations) { errorMod.Add(error.L2Norm()); configs.Add(VectorFloat.Build.DenseOfVector(configuration)); float range, stepNeg, stepPos; for (int i = 0; i <= joint; i++) { // checking coordinate constraints range = manipulator.Joints[i].CoordinateRange.X - configuration[i] * MathUtil.SIMD_DEGS_PER_RAD; stepNeg = range <= -maxStepSize ? -maxStepSize : range; range = manipulator.Joints[i].CoordinateRange.Y - configuration[i] * MathUtil.SIMD_DEGS_PER_RAD; stepPos = range >= maxStepSize ? maxStepSize : range; // generating random coordinate offset dq[i] = (stepNeg + (float)RandomThreadStatic.NextDouble() * (stepPos - stepNeg)) * MathUtil.SIMD_RADS_PER_DEG; } // get the new configuration var configurationNew = VectorFloat.Build.DenseOfVector(configuration); configurationNew.AddSubVector(dq); var fkRes = manipulator.ForwardKinematics(configurationNew); float distanceNew = fkRes.JointPositions[joint].DistanceTo(goal); if (distanceNew < distance) { // update configuration, distance and scale configuration = configurationNew; errorPos = goal - fkRes.JointPositions[joint]; error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 }); scale *= distanceNew / distance; distance = distanceNew; maxStepSize = _maxStepSize * scale; } iterations++; } return(new InverseKinematicsResult { Converged = iterations < _maxIterations && !JointLimitsExceeded(manipulator, configuration), Iterations = iterations, Configuration = configuration, Error = error }); }