/**
* <summary>Translate between joints to angles for the robotic arm</summary>
* <see cref="IRoboticModel.Translate"/>
*/
public AngleSet Translate(JointSet js)
{
AngleSet angles = new AngleSet();
ControllerJoints Elbow = (UseRightArm ? ControllerJoints.ElbowRight : ControllerJoints.ElbowLeft);
ControllerJoints Shoulder = (UseRightArm ? ControllerJoints.ShoulderRight : ControllerJoints.ShoulderLeft);
ControllerJoints Wrist = (UseRightArm ? ControllerJoints.WristRight : ControllerJoints.WristLeft);
ControllerJoints Hand = (UseRightArm ? ControllerJoints.HandRight : ControllerJoints.HandLeft);
// Shoulder joints
if (js.JointMap.ContainsKey(Elbow) &&
js.JointMap.ContainsKey(Shoulder) &&
js.JointMap.ContainsKey(Wrist) &&
js.JointMap.ContainsKey(Hand))
{
Position3d topArm = js.JointMap[Elbow] - js.JointMap[Shoulder];
angles.AngleMap.Add(RoboticAngle.ArmBaseRotate,
Math.Atan2(topArm.z, topArm.x)
);
angles.AngleMap.Add(RoboticAngle.ArmShoulderLift,
Math.Atan2(topArm.y, topArm.x)
);
Position3d bottomArm = js.JointMap[Wrist] - js.JointMap[Elbow];
double elbowangle = Math.Acos(bottomArm.Dot(topArm) / (topArm.Magnitude() * bottomArm.Magnitude()));
double elbowsign = (UseRightArm ? -1 : 1) * Math.Sign(topArm.Cross(bottomArm).z);
double elbowshift = -Math.PI / 4;
angles.AngleMap.Add(RoboticAngle.ArmElbowBend,
elbowangle * elbowsign + elbowshift
);
Position3d hand = js.JointMap[Hand] - js.JointMap[Wrist];
double wristangle = Math.Acos(bottomArm.Dot(hand) / (hand.Magnitude() * bottomArm.Magnitude()));
double wristsign = (UseRightArm ? 1 : -1) * Math.Sign(bottomArm.Cross(hand).z);
double wristshift = Math.PI / 6;
double wristmult = 2;
angles.AngleMap.Add(RoboticAngle.ArmWristTilt,
(wristangle * wristsign + wristshift) * wristmult
);
keptHand = hand;
keptTopArm = topArm;
}
if (js.JointMap.ContainsKey(Hand) &&
js.JointMap.ContainsKey(ControllerJoints.Fingertip) &&
js.JointMap.ContainsKey(ControllerJoints.Fingerstart))
{
Position3d fingertip = js.JointMap[ControllerJoints.Fingertip];
Position3d fingerstart = js.JointMap[ControllerJoints.Fingerstart];
Position3d hand = js.JointMap[Hand];
double open = (fingertip - fingerstart).Magnitude();
double fingerscale = 1;
Position3d realperp = (fingertip - hand).Cross(fingerstart - hand);
Position3d imagperp = keptHand.Cross(keptTopArm);
double rotateangle = Math.Acos(realperp.Dot(imagperp) / (realperp.Magnitude() * imagperp.Magnitude()));
double rotatesign = Math.Sign(keptHand.Dot(realperp.Cross(imagperp)));
double rotatescale = 1;
angles.AngleMap.Add(RoboticAngle.ArmWristRotate, rotateangle * rotatesign * rotatescale);
angles.AngleMap.Add(RoboticAngle.ArmHandGrasp, open * fingerscale);
}
return angles;
}
/**
* <summary>Translate between joints to angles for the robotic arm</summary>
* <see cref="IRoboticModel.Translate"/>
*/
public AngleSet Translate(JointSet js)
{
double hipdiff = js.JointMap[ControllerJoints.HipCenter].y - avghipcenter;
avghipcenter = (avghipcenter + js.JointMap[ControllerJoints.HipCenter].y) / 2;
// Find the height above for each node
Position3d lefthand = js.JointMap[ControllerJoints.HandLeft] - js.JointMap[ControllerJoints.ShoulderLeft];
Position3d righthand = js.JointMap[ControllerJoints.HandRight] - js.JointMap[ControllerJoints.ShoulderRight];
double handLeftAngle = -Math.Atan2(lefthand.y, -lefthand.x);
double handRightAngle = -Math.Atan2(righthand.y, righthand.x);
Position3d hip = js.JointMap[ControllerJoints.HipCenter];
Position3d head = js.JointMap[ControllerJoints.Head];
double dely = head.y - hip.y;
double delz = Math.Max(0,hip.z - head.z);
double bendAngle = Math.Atan2(delz, dely);
double bendPct = Math.PI/4 - (bendAngle - Math.PI/2) / (Math.PI/2);
double headAngle = bendAngle * Math.PI * 2 - Math.PI - hipdiff;
double rearAngle = -headAngle - hipdiff;
log.ErrorFormat("handRight: {0}", handRightAngle);
AngleSet angles = new AngleSet();
angles.AngleMap.Add(RoboticAngle.HeadLift, headAngle);
angles.AngleMap.Add(RoboticAngle.LeftArmLift, handLeftAngle);
angles.AngleMap.Add(RoboticAngle.RightArmLift, handRightAngle);
angles.AngleMap.Add(RoboticAngle.RearLift, rearAngle);
angles.AngleMap.Add(RoboticAngle.CurtainOpen, -Math.PI);
//log.Debug("Translating JointSet to AngleSet: as(" + angles.ToString() + ")");
return angles;
}
