public void SetJointTargetVelocityHandler(armproxy.SetJointTargetVelocity update)
{
/*
update.Body.JointName;
update.Body.TargetVelocity;
*/
}
public void ArmReliableSubscribeHandler(armproxy.ReliableSubscribe reliablesubscribe)
{
throw new NotImplementedException();
}
public void GetEndEffectorPoseHandler(armproxy.GetEndEffectorPose getendeffectorpose)
{
throw new NotImplementedException();
}
public void SetJointTargetPoseHandler(armproxy.SetJointTargetPose update)
{
string name = update.Body.JointName;
JointState j = _jointLookup[name];
/*
update.Body.TargetOrientation;
update.Body.TargetPosition;
*/
}
public void SetEndEffectorPoseHandler(armproxy.SetEndEffectorPose update)
{
Pose pose = update.Body.EndEffectorPose;
Quaternion or = pose.Orientation;
Vector3 pos = pose.Position;
}
public IEnumerator<ITask> ArmGetHandler(armproxy.Get get)
{
yield return Arbiter.Choice(
UpdateState(),
success => get.ResponsePort.Post((armproxy.ArticulatedArmState) (_state.Clone())),
ex => get.ResponsePort.Post(Fault.FromException(ex))
);
}
public void ArmSubscribeHandler(armproxy.Subscribe subscribe)
{
SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
}
/// <summary>
/// Set End Effector
/// </summary>
/// <param name="update"></param>
/// <returns></returns>
/// <remarks>Moves the arm end effector to a specified pose</remarks>
private IEnumerator<ITask> SetEndEffectorHandler(armproxy.SetEndEffectorPose update)
{
bool success = false;
float baseAngle0, shoulder0, elbow0, wrist0, rotateVal0, gripperVal0;
// Get the target joint angles for all joints
// We have to do this so that we can preserve the values for the joints
// that are NOT being changed. Note that we do not use the CURRENT joint
// angles because this could potentially stop a motion in progress.
_robotArm.GetTargetJointAngles(out baseAngle0, out shoulder0, out elbow0, out wrist0,
out rotateVal0, out gripperVal0);
// Get all the pose information required
// NOTE: The angle is assumed to be about the X axis of the gripper
Quaternion q = new Quaternion(
update.Body.EndEffectorPose.Orientation.X,
update.Body.EndEffectorPose.Orientation.Y,
update.Body.EndEffectorPose.Orientation.Z,
update.Body.EndEffectorPose.Orientation.W);
AxisAngle a = Quaternion.ToAxisAngle(q);
float p = Conversions.RadiansToDegrees(a.Angle * Math.Sign(a.Axis.X));
float x = update.Body.EndEffectorPose.Position.X;
float y = update.Body.EndEffectorPose.Position.Y;
float z = update.Body.EndEffectorPose.Position.Z;
_moveTargetEntity.Position = new xna.Vector3(x, y, z);
float baseAngle, shoulder, elbow, wrist;
// Use Inverse Kinematics to calculate the joint angles
if (!_kinematics.InverseKinematics(x, y, z, p, out baseAngle, out shoulder, out elbow, out wrist))
{
// The results were not a valid pose!
// Don't attempt the move or it might break the arm
// Should put something in the Fault!
Fault f = Fault.FromException(new Exception("No solution to Inverse Kinematics"));
update.ResponsePort.Post(f);
yield break;
}
// calculate the time needed to make the motion
float maxAngle = Math.Max(Math.Abs(baseAngle0 - baseAngle), Math.Abs(shoulder0 - shoulder));
maxAngle = Math.Max(maxAngle, Math.Abs(elbow0 - elbow));
maxAngle = Math.Max(maxAngle, Math.Abs(wrist0 - wrist));
float time = maxAngle * 11f / 360f;
// Set the arm
if (time <= 0.1f)
{
time = 0.1f;
}
yield return Arbiter.Choice(_robotArm.MoveTo(baseAngle, shoulder, elbow, wrist, rotateVal0, gripperVal0, time),
delegate { success = true; },
ShowError);
if (success)
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
else
{
// Should put something in the Fault!
Fault f = Fault.FromException(new Exception("MoveTo failed"));
update.ResponsePort.Post(f);
}
// NOT FINISHED!
// we send a replace notification since end effector pose updates affect all joints
var replace = new armproxy.Replace {Body = _state};
base.SendNotification(_submgrPort, replace);
// send an end effector update notification as well
base.SendNotification(_submgrPort, update);
yield break;
}
public void ReplaceArticulatedArmJointList(arm.ArticulatedArmState state)
{
btnConnect_ArticulatedArm.Enabled = false;
btnJointParamsApply.Enabled = true;
listArticulatedArmJoints.BeginUpdate();
try
{
listArticulatedArmJoints.Tag = state;
listArticulatedArmJoints.Items.Clear();
if (state.Joints.Count > 0)
{
UriBuilder node = new UriBuilder(state.Joints[0].State.Name);
node.Path = null;
lblActiveJointValue.Text = state.Joints[0].State.Name;
foreach (Joint j in state.Joints)
{
listArticulatedArmJoints.Items.Add(j.State.Name);
}
}
else
{
lblActiveJointValue.Text = string.Empty;
}
}
finally
{
listArticulatedArmJoints.EndUpdate();
}
}
