public IEnumerator<ITask> UpdateStateHandler(jointconfig.UpdateState update)
{
bool bFault = false;
if (bFault)
yield break;
//Console.WriteLine("BJC:UpdateStateHandler: locked = {0}", _state.Lock);
// check to see if joints are locked...
if (_state.Lock == false)
{
Vector Vdes = _state.DesiredJointAngles;
//Console.WriteLine("BJC:UpdateStateHandler: inside lock == false");
// LogInfo(LogGroups.Console, "Biclops Unlocked");
// send the command to the biclops
biclops.SetJointPositionReference r = new biclops.SetJointPositionReference();
// only send a SetJointPositionReferenceRequest if the desired position has changed.
if (newDesiredJointPositions)
{
//Console.WriteLine("BJC:UpdateStateHandler: set new joints:{0}, {1}",
// Vdes.dat[0], Vdes.dat[1]);
biclops.SetJointPositionReferenceRequest req =
new biclops.SetJointPositionReferenceRequest();
req.PositionReference = new List<double>();
req.PositionReference.Add(Vdes.dat[0]);
req.PositionReference.Add(Vdes.dat[1]);
req.SetJoint = new List<bool>();
req.SetJoint.Add(true);
req.SetJoint.Add(true);
Activate(
Arbiter.Choice(
_biclopsPort.SetJointPositionReference(req),
delegate(DefaultUpdateResponseType response)
{
//Console.WriteLine("got response from SetJointPosition");
},
delegate(W3C.Soap.Fault failure)
{
LogError("Fault posting SetJointPositionReference");
//Console.WriteLine("Fault posting SetJointPositionReference");
}));
newDesiredJointPositions = false;
}
}
else
{
// do nothing!
//Console.WriteLine("BJC:UpdateStateHandler: inside else");
}
//Console.WriteLine("BJC: querying positions");
// update the current joint angle state
biclops.QueryJointPositionsRequest jpq = new UMass.LPR.Biclops.Proxy.QueryJointPositionsRequest();
Activate(
Arbiter.Choice(
_biclopsPort.QueryJointPositions(jpq),
delegate(biclops.QueryJointPositionsResponse response)
{
//Console.WriteLine("BJC: updated joint positions: ");
//Console.WriteLine("BJC: _state.NumJoints: {0}", _state.NumJoints); // fails at this line
for (int i = 0; i < _state.NumJoints; i++)
{
//////////////////////////////////////////////////////////////////////////
//Note: biclops' pan/tilt angles are in revers to the bisight's
// the DH parameters are modified from the bisight
// there is some rotation that needs to be reversed accordingly
// but a quick fix is implemented here to reverse the angles
_state.JointAngles.dat[i] = -response.JointPositions[i];
}
},
delegate(W3C.Soap.Fault failure)
{
LogError("Fault calling QueryJointPositions");
}));
update.ResponsePort.Post(DefaultUpdateResponseType.Instance); // why are we posting a response?
yield break;
}
public virtual IEnumerator<ITask> SetLockHandler(jointconfig.SetLock request)
{
_state.Lock = request.Body.LockValue;
Console.Write("Biclops setting Lock to {0}\n", _state.Lock);
request.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator<ITask> SetJointDeltaHandler(jointconfig.SetJointDelta request)
{
Console.WriteLine("BJC: in SetJointDeltaHandler");
if (_state == null || (request.Body.JointDelta.m != _state.NumJoints))
{
request.ResponsePort.Post(new Fault());
}
else
{
// set the joint delta, and add it to cur for desired
// worry about scaling when we send to the biclops
Vector Vdes = _state.DesiredJointAngles;
Vector Vcur = _state.JointAngles;
Vector Vu = request.Body.JointDelta;
Vcur.add(ref Vu, ref Vdes);
Console.WriteLine("Vdes:");
Vdes.display();
Console.WriteLine("_state.DesiredJointAngles:");
_state.DesiredJointAngles.display();
newDesiredJointPositions = true;
}
request.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public IEnumerator<ITask> ReplaceHandler(jointconfig.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
public IEnumerator<ITask> GetJointConfigurationHandler(jointconfig.GetJointConfiguration query)
{
if (_state.JointAngles == null)
{
LogError("BiclopsJointCongfiguration -- can't return JointAngles!!");
query.ResponsePort.Post(new Fault());
}
else
{
query.ResponsePort.Post(_state.JointAngles);
}
yield break;
}
public virtual IEnumerator<ITask> GetHandler(jointconfig.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
