protected override VectorFloat GetCoordinateOffset(Matrix <float> jacobian, VectorFloat error) { // get Jacobian pseudoinverse var JP = jacobian.PseudoInverse(); // calculate the displacement return(-JP * error); // TODO: check why minus should always be presented for dq in these methods! }
public Node(Node parent, Vector3 point, VectorFloat q) { Parent = parent; Childs = new List <Node>(); Point = point; this.q = q; }
public void dmpGetYawPitchRoll(float[] data, Quaternion q, VectorFloat gravity) { // yaw: (about Z axis) data[0] = (float)Math.Atan2(2 * q.x * q.y - 2 * q.w * q.z, 2 * q.w * q.w + 2 * q.x * q.x - 1); // pitch: (nose up/down, about Y axis) data[1] = (float)Math.Atan(gravity.x / Math.Sqrt(gravity.y * gravity.y + gravity.z * gravity.z)); // roll: (tilt left/right, about X axis) data[2] = (float)Math.Atan(gravity.y / Math.Sqrt(gravity.x * gravity.x + gravity.z * gravity.z)); }
// uint8_t dmpGetGyroAndAccelSensor(long *data, uint8_t* packet); // uint8_t dmpGetGyroSensor(long *data, uint8_t* packet); // uint8_t dmpGetControlData(long *data, uint8_t* packet); // uint8_t dmpGetTemperature(long *data, uint8_t* packet); // uint8_t dmpGetGravity(long *data, uint8_t* packet); public VectorFloat dmpGetGravity(Quaternion q) { VectorFloat v = new VectorFloat(); v.x = 2 * (q.x * q.z - q.w * q.y); v.y = 2 * (q.w * q.x + q.y * q.z); v.z = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z; return(v); }
public VectorFloat Filter(VectorFloat v, float dt, float hz) { float alpha = FilterUtil.Alpha(hz,dt); VectorFloat last = v * alpha + current * (1.0f - alpha); current.x = last.x; current.y = last.y; current.z = last.z; return last; }
protected override VectorFloat GetCoordinateOffset(Matrix <float> jacobian, VectorFloat error) { // get Jacobian transpose and an identity matrix var jacobianTranspose = jacobian.Transpose(); var identity = Matrix <float> .Build.DenseIdentity(error.Count); // calculate the displacement return(-jacobianTranspose * (jacobian * jacobianTranspose + _damping * _damping * identity).Solve(error)); }
public VectorFloat Filter(VectorFloat v, float dt, float hz_x, float hz_y, float hz_z) { VectorFloat alpha = new VectorFloat(FilterUtil.Alpha(hz_x,dt), FilterUtil.Alpha(hz_y,dt), FilterUtil.Alpha(hz_z,dt)); VectorFloat last = v * alpha + current * (new VectorFloat(){x=1,y=1,z=1} - alpha); current.x = last.x; current.y = last.y; current.z = last.z; return last; }
// uint8_t dmpSetLinearAccelFilterCoefficient(float coef); // uint8_t dmpGetLinearAccel(long *data, uint8_t* packet); VectorInt16 dmpGetLinearAccel(VectorInt16 vRaw, VectorFloat gravity) { // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) VectorInt16 v = new VectorInt16(); v.x = (short)(vRaw.x - gravity.x * 4096); v.y = (short)(vRaw.y - gravity.y * 4096); v.z = (short)(vRaw.z - gravity.z * 4096); return(v); }
protected bool ErrorExceedsThreshold(Manipulator manipulator, VectorFloat configuration, Vector3 goal, int joint, out ForwardKinematicsResult fkRes, out VectorFloat error) { fkRes = manipulator.ForwardKinematics(configuration); var errorPos = goal - fkRes.JointPositions[joint]; // TODO: integrate orientation goal somehow? error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 }); return(errorPos.Length() > _threshold); }
public VectorFloat Update(VectorFloat error, float dtime) { error_integral += error * dtime; error_derivate = (error - previous_error) / dtime; previous_error = error; error_integral.x = MathUtil.Clamp(error_integral.x, -I_max.x, I_max.x); error_integral.y = MathUtil.Clamp(error_integral.y, -I_max.y, I_max.y); error_integral.z = MathUtil.Clamp(error_integral.z, -I_max.z, I_max.z); return error * P + error_integral * I + error_derivate * D; }
protected bool JointLimitsExceeded(Manipulator manipulator, VectorFloat configuration) { for (int i = 0; i < manipulator.Joints.Length; i++) { if (configuration[i] < manipulator.Joints[i].CoordinateRange.X * MathUtil.SIMD_RADS_PER_DEG || configuration[i] > manipulator.Joints[i].CoordinateRange.Y * MathUtil.SIMD_RADS_PER_DEG) { return(true); } } return(false); }
public override InverseKinematicsResult Execute(Manipulator manipulator, Vector3 goal, int joint = -1) { // use gripper if default joint if (joint == -1) { joint = manipulator.Joints.Length - 1; } VectorFloat configuration = manipulator.q, dq; VectorFloat error; // TODO: check for oscillations (the error starts increasing) and break if they appear int iterations = 0; while (ErrorExceedsThreshold(manipulator, configuration, goal, joint, out ForwardKinematicsResult fkRes, out error) && iterations < _maxIterations) { errorMod.Add(error.L2Norm()); configs.Add(VectorFloat.Build.DenseOfVector(configuration)); // get generalized coordinates' offset dq = GetCoordinateOffset(CreateJacobian(fkRes, joint), error); // update configuration configuration.AddSubVector(dq); //for (int i = 0; i < manipulator.Joints.Length; i++) //{ // manipulator.Joints[i].InitialCoordinate = configuration[i]; //} iterations++; } return(new InverseKinematicsResult { Converged = iterations < _maxIterations && !JointLimitsExceeded(manipulator, configuration), Iterations = iterations, Configuration = configuration, Error = error }); }
} // TODO: for unified usage of path consider replacing with "object Context" or something similar public Node(Node parent, Vector3[] points, VectorFloat q) { // assign the parent to the current node's parent Parent = parent; if (parent != null) { if (parent.Child != null) { // assign the parent's child to the current node's child Child = parent.Child; // assign the current node to the child's parent Child.Parent = this; } // assign the current node to the parent's child parent.Child = this; } Points = points; this.q = q; }
public void setTranslation ( VectorFloat vectorFloat ) { setTranslation( vectorFloat.Data ); }
public void addTranslation ( VectorFloat vectorFloat ) { addTranslation( vectorFloat.Data ); }
protected abstract VectorFloat GetCoordinateOffset(Matrix <float> jacobian, VectorFloat error);
public void ChangeNode(Node node, Vector3[] points, VectorFloat config) { node.Change(points, config); ChgBuffer.Enqueue(node); }
// uint8_t dmpSetLinearAccelFilterCoefficient(float coef); // uint8_t dmpGetLinearAccel(long *data, uint8_t* packet); VectorInt16 dmpGetLinearAccel(VectorInt16 vRaw, VectorFloat gravity) { // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) VectorInt16 v = new VectorInt16(); v.x = (short)(vRaw.x - gravity.x*4096); v.y = (short)(vRaw.y - gravity.y*4096); v.z = (short)(vRaw.z - gravity.z*4096); return v; }
public VectorFloat Filter(VectorFloat v, float dt, float hz) { return Filter(v, dt, hz, hz, hz); }
public void Update(float dtime) { motion = mpu.GetMotion6(); // these methods (and a few others) are also available //accelgyro.getAcceleration(&ax, &ay, &az); //accelgyro.getRotation(&gx, &gy, &gz); // display accel/gyro x/y/z values //Console.WriteLine("a/g: {0} {1} {2} {3} {4} {5}", m.ax, m.ay, m.az, m.gx, m.gy, m.gz); if (use_dmp && dmpReady) { ushort fifoCount = mpu.GetFIFOCount(); if (fifoCount == 1024) { // reset so we can continue cleanly mpu.ResetFIFO(); _logger.Debug("MPU-6050 IMU FIFO overflow"); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (fifoCount >= 42) { // read a packet from FIFO mpu.GetFIFOBytes(fifoBuffer, (byte)packetSize); q = mpu.DmpGetQuaternion(fifoBuffer); gravity = mpu.DmpGetGravity(q); mpu.DmpGetYawPitchRoll(ypr, q, gravity); //Console.WriteLine("quat {0} {1} {2} {3} ", q.w, q.x, q.y, q.z); //Console.WriteLine("ypr {0} {1} {2} ", ypr[0] * 180 / Math.PI, ypr[1] * 180 / Math.PI, ypr[2] * 180 / Math.PI); } } }
protected override PathPlanningResult RunAbstract(Manipulator manipulator, Vector3 goal, InverseKinematicsSolver solver, CancellationToken cancellationToken) { if (manipulator.DistanceTo(goal) < _threshold) { // the goal is already reached return new PathPlanningResult { Iterations = 0, Path = null } } ; _manipulator = manipulator; _initialConfiguration = manipulator.q; _solver = solver; _goal = goal; var generation = new List <Chromosome>(); // get initial solution var bezierInitial = ConstructBezier(_manipulator.GripperPos, goal, _bezierControlPointsCount); var pathInitial = ConstructPath(bezierInitial, _bezierStep); var chromosomeInitial = new Chromosome(bezierInitial, pathInitial, Rate(pathInitial)); generation.Add(chromosomeInitial); Dominant = chromosomeInitial; Changed = true; // evolve generations Console.SetCursorPosition(0, 10); Console.WriteLine("Starting genetic algorithm..."); Iterations = 0; while (generation[0].Weight > 0 && Iterations++ < _maxIterations) { cancellationToken.ThrowIfCancellationRequested(); // get new generation generation = Evolve(generation, _offspringSize, _survivalSize); Console.SetCursorPosition(0, 11); Console.WriteLine($"Generations passed: {Iterations}"); Console.WriteLine($"Best fit: {generation[0].Weight}"); if (!Locked) { Dominant = generation[0]; // TODO: it seems like previous dominant is not disposed properly; fix!! Changed = true; } } Console.SetCursorPosition(0, 15); Console.WriteLine("Found path's Bezier curve:"); foreach (var point in generation[0].BezierCurve.Points) { Console.WriteLine(point); } var path = generation[0].Path; path.SetModel(); return(new PathPlanningResult { Iterations = Iterations - 1, Path = path }); }
public void Change(Vector3[] point, VectorFloat config) { Points = point; q = config; }
public VectorFloat Filter(VectorFloat v, float dt) { integral += v * dt; return integral; }
// uint8_t dmpGetGyroAndAccelSensor(long *data, uint8_t* packet); // uint8_t dmpGetGyroSensor(long *data, uint8_t* packet); // uint8_t dmpGetControlData(long *data, uint8_t* packet); // uint8_t dmpGetTemperature(long *data, uint8_t* packet); // uint8_t dmpGetGravity(long *data, uint8_t* packet); public VectorFloat dmpGetGravity(Quaternion q) { VectorFloat v = new VectorFloat(); v.x = 2 * (q.x*q.z - q.w*q.y); v.y = 2 * (q.w*q.x + q.y*q.z); v.z = q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z; return v; }
public VectorFloat Filter(VectorFloat v, float dt) { if (first) { first = false; last = v; } VectorFloat ret = (v - last) / dt; last = v; return ret; }
public void Calibrate() { gyro_calibration = GetUncalibratedGyro() * -1; accel_calibration = new VectorFloat(0, 0, 1) - GetUncalibratedAccel(); }
public void AddLast(Vector3[] points, VectorFloat configuration) { AddNode(new Node(Last, points, configuration)); }
public VectorFloat Filter(VectorFloat v, float dt, float hz_x, float hz_y, float hz_z) { if(first) { first = false; last = v; } VectorFloat d = (v - last) / dt; last = v; VectorFloat alpha = new VectorFloat(FilterUtil.Alpha(hz_x,dt), FilterUtil.Alpha(hz_y,dt), FilterUtil.Alpha(hz_z,dt)); current = alpha * (current + d); return current; }
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 }); }