public void Dispose()
{
ActionFrames?.Clear();
ActionFrames = null;
JointAngles?.Clear();
JointAngles = null;
KeyBones?.Clear();
KeyBones = null;
}
private JointAngles GetJointAngles(double x, double y, double z)
{
z = z + RobotArm.EndEffectorLength;
Vector3 wristLocation = new Vector3(x, y, z);
JointAngles jointAngles = RobotArm.DoInverseKinematics(wristLocation);
jointAngles.WristTiltAngle = GetWristTiltAngleForVertical(jointAngles);
return(jointAngles);
}
private JointAngles GetJointAngles(double x, double y, double z, bool zIsBasedOnEndeffectorTip)
{
if (zIsBasedOnEndeffectorTip)
{
z = z + RobotArm.EndEffectorLength;
}
Vector3 wristLocation = new Vector3(x, y, z);
JointAngles jointAngles = RobotArm.DoInverseKinematics(wristLocation);
return(jointAngles);
}
private void Recalculate()
{
m_JointAngles = GetJointAngles(m_X, m_Y, m_Z, this.ZIsBasedOnEndeffectorTip);
OnPropertyChanged("BaseAnglePhi0");
OnPropertyChanged("BaseAngleTheta0");
OnPropertyChanged("BaseMilliSecs");
OnPropertyChanged("ShoulderAnglePhi1");
OnPropertyChanged("ShoulderAngleTheta1");
OnPropertyChanged("ShoulderMilliSecs");
OnPropertyChanged("ElbowAnglePhi2");
OnPropertyChanged("ElbowAngleTheta2");
OnPropertyChanged("ElbowMilliSecs");
}
private void MoveToDropTarget()
{
Debug.WriteLine("Moving to drop target.");
JointAngles jointAngles = GetJointAngles(m_DropOffLocation.X, m_DropOffLocation.Y, m_DropOffLocation.Z);
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
this.SSC32Board.RunCommand(moveCommand);
}
private void MoveDownToPickupLocation(Vector3 pickupLocation)
{
Debug.WriteLine("Moving to pickup location.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(pickupLocation.X, pickupLocation.Y, pickupLocation.Z);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
this.SSC32Board.RunCommand(moveCommand);
}
private void MoveToDropTarget()
{
Debug.WriteLine("Moving to drop target.");
Vector3 wristLocation = new Vector3(50, 150, 150);
JointAngles jointAngles = GetJointAngles(50, 150, 40, true);
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = GetWristTiltAngleForVertical(jointAngles);
//moveCommand[ChannelId.Gripper] = 28;
m_SSC32Board.RunCommand(moveCommand);
}
private void MoveToPickupLocation()
{
Debug.WriteLine("Moving to pikup location.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(this.X, this.Y, this.Z, this.ZIsBasedOnEndeffectorTip);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = GetWristTiltAngleForVertical(jointAngles);
//moveCommand[ChannelId.Gripper] = 20;
m_SSC32Board.RunCommand(moveCommand);
}
public async Task <string> SetPoseAsync(int value, MotionType type = MotionType.JOINT, float maxVelocity = 2)
{
var response = await _connection.PutPose(JointAngles.ToArray(), value, type, maxVelocity);
switch (response.StatusCode)
{
case HttpStatusCode.OK:
return("OK");
case HttpStatusCode.PreconditionFailed:
return(await response.Content.ReadAsStringAsync());
default:
return("Robot does not respond");
}
}
private void MoveAboveHomePosition()
{
Debug.WriteLine("Moving above home position.");
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(m_HomeLocation.X, m_HomeLocation.Y, m_HomeLocation.Z + 30);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
moveCommand[ChannelId.Gripper] = c_OpenGripperPosition;
this.SSC32Board.RunCommand(moveCommand);
}
private void MoveTo(Vector3 location, string message)
{
if (!String.IsNullOrEmpty(message))
{
Debug.WriteLine(message);
}
MoveLogicalCommand moveCommand = new MoveLogicalCommand(m_Servos);
JointAngles jointAngles = GetJointAngles(location.X, location.Y, location.Z);
moveCommand[ChannelId.Base] = jointAngles.BaseAngle;
moveCommand[ChannelId.Shoulder] = jointAngles.ShoulderAngle;
moveCommand[ChannelId.Elbow] = jointAngles.ElbowAngle;
moveCommand[ChannelId.WristUpDown] = jointAngles.WristTiltAngle;
this.SSC32Board.RunCommand(moveCommand);
}
static void Main(string[] args)
{
try
{
Vector3 wristLocation = new Vector3(RobotArm.ForearmLength, 0, RobotArm.ShoulderHeight + RobotArm.UpperArmLength);
JointAngles jointAngles = RobotArm.DoInverseKinematics(wristLocation);
Debug.WriteLine(jointAngles);
//DHFrameInfo[] frameInfos = new DHFrameInfo[]
// {
// new DHFrameInfo(0, 90, 60, 90),
// new DHFrameInfo(100, 0, 0, 90),
// //new DHFrameInfo(3, 0, 0, 0),
// };
//Matrix4x4[] transformations = new Matrix4x4[frameInfos.Length];
//Matrix4x4 completeTransformation = Matrix4x4.Identity;
//for (int i = frameInfos.Length - 1; i >= 0; i--)
//{
// transformations[i] = DHTransformation.ToPreviousFrame(frameInfos[i]);
// completeTransformation = transformations[i] * completeTransformation;
//}
//Vector4 vector1 = new Vector4(0, 0, 0);
//Vector4 transformedVector = completeTransformation * vector1;
//System.Console.WriteLine(transformedVector);
}
catch (Exception ex)
{
System.Console.WriteLine(ex.ToString());
}
finally
{
System.Console.ReadLine();
}
}
private double GetWristTiltAngleForVertical(JointAngles jointAngles)
{
return(-90 + jointAngles.ShoulderAngle - jointAngles.ElbowAngle);
}
private double GetWristTiltAngleForVertical(JointAngles jointAngles)
{
return -90 + jointAngles.ShoulderAngle - jointAngles.ElbowAngle;
}
private void Recalculate()
{
m_JointAngles = GetJointAngles(m_X, m_Y, m_Z, this.ZIsBasedOnEndeffectorTip);
OnPropertyChanged("BaseAnglePhi0");
OnPropertyChanged("BaseAngleTheta0");
OnPropertyChanged("BaseMilliSecs");
OnPropertyChanged("ShoulderAnglePhi1");
OnPropertyChanged("ShoulderAngleTheta1");
OnPropertyChanged("ShoulderMilliSecs");
OnPropertyChanged("ElbowAnglePhi2");
OnPropertyChanged("ElbowAngleTheta2");
OnPropertyChanged("ElbowMilliSecs");
}