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