protected override void Start() { // needed for HttpPost _httpUtilities = DsspHttpUtilitiesService.Create(Environment); if (_state == null) { //int c = 0; LogInfo("Creating new BiclopsRightCameraJointConfiguration State..."); // get state _state = new jointconfig.JointConfigurationState(); _state.NumLinks = 8; // this'll need to get updated accordingly _state.NumJoints = 2; _state.Moveable = true; _state.JointAngles = new Vector(_state.NumJoints); _state.DesiredJointAngles = new Vector(_state.NumJoints); _state.MinLimits = new Vector(_state.NumJoints); _state.MaxLimits = new Vector(_state.NumJoints); _state.MaxDelta = new Vector(_state.NumJoints); _state.LinkTypes = new List<string>(); _state.DHParams = new List<List<double>>(); // DH params will of course be totally different...but we'll need to add them in a similar fashion. /* 0.000000 -0.01 0.412 0.000000 REVOLUTE -1.570796 0.00 0.000 0.000000 REVOLUTE 1.570796 0.00 0.005 0.000000 CONSTANT -1.570796 0.00 0.000 0.000000 CONSTANT 1.570796 0.00 0.000 -1.570796 CONSTANT -1.570796 0.00 0.000 0.000000 CONSTANT 0.000000 0.00 0.040 0.000000 CONSTANT 0.000000 0.09 0.000 0.000000 CONSTANT */ //0 AddDHParameter(0, -0.01, 0.412, 0, "LINK_TYPE_REVOLUTE"); //1 AddDHParameter(-Math.PI / 2.0, 0, 0, 0, "LINK_TYPE_REVOLUTE"); //2 AddDHParameter(Math.PI / 2.0, 0, 0.005, 0, "LINK_TYPE_CONSTANT"); //3 AddDHParameter(-Math.PI / 2.0, 0, 0, 0, "LINK_TYPE_CONSTANT"); //4 AddDHParameter(Math.PI / 2.0, 0, 0, -Math.PI / 2.0, "LINK_TYPE_CONSTANT"); //5 AddDHParameter(-Math.PI / 2.0, 0, 0, 0, "LINK_TYPE_CONSTANT"); //6 AddDHParameter(0, 0, 0.04, 0, "LINK_TYPE_CONSTANT"); //7 AddDHParameter(0, 0.09, 0, 0, "LINK_TYPE_CONSTANT"); // we'll need to fix these, too _state.MinLimits.dat[0] = -2.0; _state.MinLimits.dat[1] = -0.35; _state.MaxLimits.dat[0] = 2.0; _state.MaxLimits.dat[1] = 1.2; _state.RobotName = "BiclopsRight"; _state.Lock = true; //set up default vals for (int i = 0; i < 2; i++) { _state.JointAngles.dat[i] = 0; _state.DesiredJointAngles.dat[i] = 0; _state.MaxDelta.dat[i] = 2.0; // because we're sending joint position references to drivers that already have reasonable // PD gains, we don't need to worry about max delta here (so 2 radians per cycle is fine) } for (int i = 0; i < _state.NumLinks; i++) { Console.Write("DH[{0}]=({1:F2} {2:F2} {3:F2} {4:F2})\n", i, _state.DHParams[i][0], _state.DHParams[i][1], _state.DHParams[i][2], _state.DHParams[i][3]); } for (int i = 0; i < _state.NumJoints; i++) { Console.Write("Range[{0}]=({1:F2} {2:F2})\n", i, _state.MinLimits.dat[i], _state.MaxLimits.dat[i]); } for (int i = 0; i < _state.NumJoints; i++) { Console.Write("MaxDelta[{0}]={1:F2}\n", i, _state.MaxDelta.dat[i]); } base.SaveState(_state); } _state.Moveable = true; _state.Lock = true; // // Add service specific initialization here // SpawnIterator(ConnectToBiclopsService); base.Start(); Console.WriteLine("Starting Biclops Right Camera JointConfiguration Thread"); // start main update loop _waitPort.Post(DateTime.Now); Activate(Arbiter.Receive(true, _waitPort, UpdateLoopHandler)); }
public IEnumerator<ITask> ReplaceHandler(jointconfig.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; }