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