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