private void InitFingers()
{
System.Console.WriteLine("JACO API InitFingers()");
//An object that represents the state of a joystick.
CJoystickValue Joystick = new CJoystickValue();
CControlMappingCharts mapping = m_Arm.ConfigurationsManager.GetControlMappingCharts();
//We set the mapping system in a mode that we need.
mapping.ActualControlMapping = 2;
mapping.ControlMapping[2].ActualModeA = 1;
mapping.ControlMapping[2].ActualModeB = -1;
//We set our mapping configuration
m_Arm.ConfigurationsManager.SetControlMappingCharts(mapping);
//We verify that the change has been made correctly(only for learning purpose :) )
mapping = m_Arm.ConfigurationsManager.GetControlMappingCharts();
//We show informations on screen to make that we execute the right functionality
System.Console.WriteLine("Current mapping = " + mapping.ActualControlMapping);
System.Console.WriteLine("Actual mode A = " + mapping.ControlMapping[2].ActualModeA);
System.Console.WriteLine("Mode A 1 - INCLINE LR : " + (CJacoStructures.ControlFunctionnalityValues)mapping.ControlMapping[2].ModeControlsA[1].ControlSticks[(int)CJacoStructures.ControlStickValues.InclineLR].Plus);
//We tell Jaco that from now on, the API is taking control.
//m_Arm.ControlManager.StartControlAPI();
//We set the INCLINELR stick's Value of our vitual joystick to -1(like someone was fully pressing the stick to the right)
Joystick.InclineLR = -1f;
//We send the command long enough for the fingers to be initialized.
for (int i = 0; i < 100; i++)
{
System.Console.WriteLine("Looping for finger init i=" + i);
try
{
m_Arm.ControlManager.SendJoystickFunctionnality(Joystick);
}
catch (Exception ex)
{
System.Console.WriteLine("Exception = " + ex.GetType());
}
Thread.Sleep(10);
}
//We reset the joystick value.(like the person release the stick)
Joystick.InclineLR = 0;
//We send it to Jaco
m_Arm.ControlManager.SendJoystickFunctionnality(Joystick);
}
public void Retract()
{
System.Console.WriteLine(
"JACO API Retract()");
//Retract again...
m_Cmd = new CJoystickValue();
m_Cmd.ButtonValue[2] = 1;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
m_IsRetracting = true;
}
private void InitSequence()
{
//Initialize fingers
//InitFingers();
m_Cmd = new CJoystickValue();
m_Cmd.ButtonValue[2] = 1; // Retract/Ready button.
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
// The button release will be done in the update() method at the
// appropriate time.
m_LastCmd = DateTime.Now;
m_RetractDelay = new TimeSpan(0, 0, 0, 24, 0); // 8 secs.
m_IsRetracting = true;
}
static void Main(string[] args)
{
bool dll = args.Length > 0 && String.Compare(args[0], "dll") == 0;
if (dll)
{
_error = "#ERROR [JACO]: ";
_info = "#INFO [JACO]: ";
_warning = "#WARNING [JACO]: ";
}
try {
_jaco = new CJacoArm(Crypto.GetInstance().Encrypt("C6H12O6h2so4"));
Console.WriteLine(_info + "Successfully connected to Jaco.");
} catch (CAccessDeniedException) {
Console.WriteLine(_error + "Access to Jaco denied. Please check password and USB connection.");
return;
} catch (Exception) {
Console.WriteLine(_error + "Could not connect to Jaco.");
return;
}
_cmd = new CJoystickValue();
_zones = _jaco.ConfigurationsManager.GetProtectionZones();
_jaco.ControlManager.StartControlAPI();
_api = true;
_data = new float[37];
for (int i = 0; i < 37; i++)
{
_data[i] = 0f;
}
if (dll)
{
Console.WriteLine(_info + "Active protection zones: " + _zones.NbZoneActive);
return;
}
TerminalCommand(args);
Deinit();
}
public JacoArm(string key)
{
m_IsEnabled = false;
m_State = new JacoArmState();
m_Cmd = new CJoystickValue();
try
{
CCypherMessage pass = Crypto.GetInstance().Encrypt(key);
m_Arm = new CJacoArm(pass);
Console.WriteLine("Wrapper initialized.");
if (m_Arm.JacoIsReady())
{
m_Arm.ControlManager.StartControlAPI();
// Init sequence, place the arm in READY position.
InitSequence();
Console.WriteLine("The arm is ready.");
m_IsEnabled = true;
}
else
{
Console.WriteLine("The arm is not ready!");
m_IsEnabled = false;
}
} catch (Exception e)
{
System.Console.WriteLine(
"JACO API initialisation failed. Reason: ");
System.Console.WriteLine(e.ToString());
m_IsEnabled = false;
}
// 200 ms delay.
m_WatchDogDelay = new TimeSpan(0, 0, 0, 0, 200);
}
private void InitSequence()
{
//Initialize fingers
//InitFingers();
m_Cmd = new CJoystickValue();
m_Cmd.ButtonValue[2] = 1; // Retract/Ready button.
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
// The button release will be done in the update() method at the
// appropriate time.
m_LastCmd = DateTime.Now;
m_RetractDelay = new TimeSpan(0,0,0,24,0); // 8 secs.
m_IsRetracting = true;
}
private void InitFingers()
{
System.Console.WriteLine("JACO API InitFingers()");
//An object that represents the state of a joystick.
CJoystickValue Joystick = new CJoystickValue();
CControlMappingCharts mapping = m_Arm.ConfigurationsManager.GetControlMappingCharts();
//We set the mapping system in a mode that we need.
mapping.ActualControlMapping = 2;
mapping.ControlMapping[2].ActualModeA = 1;
mapping.ControlMapping[2].ActualModeB = -1;
//We set our mapping configuration
m_Arm.ConfigurationsManager.SetControlMappingCharts(mapping);
//We verify that the change has been made correctly(only for learning purpose :) )
mapping = m_Arm.ConfigurationsManager.GetControlMappingCharts();
//We show informations on screen to make that we execute the right functionality
System.Console.WriteLine("Current mapping = " + mapping.ActualControlMapping);
System.Console.WriteLine("Actual mode A = " + mapping.ControlMapping[2].ActualModeA);
System.Console.WriteLine("Mode A 1 - INCLINE LR : " + (CJacoStructures.ControlFunctionnalityValues)mapping.ControlMapping[2].ModeControlsA[1].ControlSticks[(int)CJacoStructures.ControlStickValues.InclineLR].Plus);
//We tell Jaco that from now on, the API is taking control.
//m_Arm.ControlManager.StartControlAPI();
//We set the INCLINELR stick's Value of our vitual joystick to -1(like someone was fully pressing the stick to the right)
Joystick.InclineLR = -1f;
//We send the command long enough for the fingers to be initialized.
for (int i = 0; i < 100; i++ )
{
System.Console.WriteLine("Looping for finger init i=" + i);
try
{
m_Arm.ControlManager.SendJoystickFunctionnality(Joystick);
}
catch(Exception ex)
{
System.Console.WriteLine("Exception = " + ex.GetType());
}
Thread.Sleep(10);
}
//We reset the joystick value.(like the person release the stick)
Joystick.InclineLR = 0;
//We send it to Jaco
m_Arm.ControlManager.SendJoystickFunctionnality(Joystick);
}
/// \brief Update the internal state, send commands.
public void Update()
{
CVectorAngle jp = m_Arm.ConfigurationsManager.GetJointPositions();
CVectorEuler hand = m_Arm.ConfigurationsManager.GetHandPosition();
// See the DH specs for angle conversions.
m_State.shoulder_yaw.angle =
(180.0 - jp.Angle[0]) / (180.0 / Math.PI);
m_State.shoulder_pitch.angle =
(jp.Angle[1] - 270.0) / (180.0 / Math.PI);
m_State.elbow_pitch.angle =
(90.0 - jp.Angle[2]) / (180.0 / Math.PI);
m_State.elbow_roll.angle =
(180.0 - jp.Angle[3]) / (180.0 / Math.PI);
m_State.wrist_roll.angle =
(180.0 - jp.Angle[4]) / (180.0 / Math.PI);
m_State.hand_roll.angle =
(260.0 - jp.Angle[5]) / (180.0 / Math.PI);
//Hand pose & orientation
m_State.hand_position_x = hand.Position[0];
m_State.hand_position_y = hand.Position[1];
m_State.hand_position_z = hand.Position[2];
m_State.hand_orientation_x = hand.Rotation[0];
m_State.hand_orientation_y = hand.Rotation[1];
m_State.hand_orientation_z = hand.Rotation[2];
//Update finger information (slow)
float[] fingerPos = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().UserPosition.FingerPosition;
//Update finger angles
m_State.finger_1.angle = fingerPos[0] * Math.PI / 180.0;
m_State.finger_2.angle = fingerPos[1] * Math.PI / 180.0;
m_State.finger_3.angle = fingerPos[2] * Math.PI / 180.0;
//if (m_IsRetracting && ((DateTime.Now - m_LastCmd) > m_RetractDelay))
if(m_IsRetracting)
{
try
{
//Getting position Live Only when retracting and wait for
CPosition positionLive = new CPosition();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
//System.Console.WriteLine("Retract status ");
//System.Console.WriteLine(positionLive.SystemStatus.RetractStatus);
/*
Mode_Normal_To_Ready
Mode_Ready_Standby
Mode_Ready_To_Retract
Mode_Retract_Standby
Mode_Retract_To_Ready
Mode_Normal
Mode_NoInit_To_Ready
ERROR
*/
//Are we done retracting
if (
positionLive.SystemStatus.RetractStatus == 1 ||
positionLive.SystemStatus.RetractStatus == 3)
{
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
else
{
//Look for timeout
if ((DateTime.Now - m_LastCmd) > m_RetractDelay)
{
System.Console.WriteLine("Retract delay expired");
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception e)
{
System.Console.WriteLine(
"JACO API diagnostic API Failed : ");
System.Console.WriteLine(e.ToString());
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
return;
}
// 200 ms watchdog, reset the command if it's too old.
if ((DateTime.Now - m_LastCmd) > m_WatchDogDelay)
m_Cmd = new CJoystickValue();
// NOTE: Disabled for now, testing trajectory mode.
//m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
}
public void Retract()
{
System.Console.WriteLine(
"JACO API Retract()");
//Retract again...
m_Cmd = new CJoystickValue();
m_Cmd.ButtonValue[2] = 1;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
m_IsRetracting = true;
}
public JacoArm(string key)
{
m_IsEnabled = false;
m_State = new JacoArmState();
m_Cmd = new CJoystickValue();
try
{
CCypherMessage pass = Crypto.GetInstance().Encrypt(key);
m_Arm = new CJacoArm(pass);
Console.WriteLine("Wrapper initialized.");
if (m_Arm.JacoIsReady())
{
m_Arm.ControlManager.StartControlAPI();
// Init sequence, place the arm in READY position.
InitSequence();
Console.WriteLine("The arm is ready.");
m_IsEnabled = true;
}
else
{
Console.WriteLine("The arm is not ready!");
m_IsEnabled = false;
}
} catch (Exception e)
{
System.Console.WriteLine(
"JACO API initialisation failed. Reason: ");
System.Console.WriteLine(e.ToString());
m_IsEnabled = false;
}
// 200 ms delay.
m_WatchDogDelay = new TimeSpan(0,0,0,0,200);
}
public void JacoRetract()
{
try
{
System.Console.WriteLine("Jaco arm API Retract()");
m_Cmd = new CJoystickValue();
m_Cmd.ButtonValue[2] = 1;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
while(m_IsRetracting)
{
try
{
//Getting position Live Only when retracting and wait for
positionLive = new CPosition();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
System.Console.WriteLine("Retract status ");
System.Console.WriteLine(positionLive.SystemStatus.RetractStatus);
//Are we done retracting
if ( positionLive.SystemStatus.RetractStatus == 1 || positionLive.SystemStatus.RetractStatus == 3)
{
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
m_LastCmd = DateTime.Now;
}
else
{
//Look for timeout
if ((DateTime.Now - m_LastCmd) > m_RetractDelay)
{
System.Console.WriteLine("Retract delay expired");
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception e)
{
System.Console.WriteLine("JACO retract diagnostic API Failed : ");
System.Console.WriteLine(e.ToString());
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception ex)
{
System.Console.WriteLine("EXCEPTION in ");
System.Console.WriteLine(ex.ToString());
}
}
/// \brief Update the internal state, send commands.
public void Update()
{
CVectorAngle jp = m_Arm.ConfigurationsManager.GetJointPositions();
CVectorEuler hand = m_Arm.ConfigurationsManager.GetHandPosition();
// See the DH specs for angle conversions.
m_State.shoulder_yaw.angle =
(180.0 - jp.Angle[0]) / (180.0 / Math.PI);
m_State.shoulder_pitch.angle =
(jp.Angle[1] - 270.0) / (180.0 / Math.PI);
m_State.elbow_pitch.angle =
(90.0 - jp.Angle[2]) / (180.0 / Math.PI);
m_State.elbow_roll.angle =
(180.0 - jp.Angle[3]) / (180.0 / Math.PI);
m_State.wrist_roll.angle =
(180.0 - jp.Angle[4]) / (180.0 / Math.PI);
m_State.hand_roll.angle =
(260.0 - jp.Angle[5]) / (180.0 / Math.PI);
//Hand pose & orientation
m_State.hand_position_x = hand.Position[0];
m_State.hand_position_y = hand.Position[1];
m_State.hand_position_z = hand.Position[2];
m_State.hand_orientation_x = hand.Rotation[0];
m_State.hand_orientation_y = hand.Rotation[1];
m_State.hand_orientation_z = hand.Rotation[2];
//Update finger information (slow)
float[] fingerPos = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco().UserPosition.FingerPosition;
//Update finger angles
m_State.finger_1.angle = fingerPos[0] * Math.PI / 180.0;
m_State.finger_2.angle = fingerPos[1] * Math.PI / 180.0;
m_State.finger_3.angle = fingerPos[2] * Math.PI / 180.0;
//if (m_IsRetracting && ((DateTime.Now - m_LastCmd) > m_RetractDelay))
if (m_IsRetracting)
{
try
{
//Getting position Live Only when retracting and wait for
CPosition positionLive = new CPosition();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
//System.Console.WriteLine("Retract status ");
//System.Console.WriteLine(positionLive.SystemStatus.RetractStatus);
/*
* Mode_Normal_To_Ready
* Mode_Ready_Standby
* Mode_Ready_To_Retract
* Mode_Retract_Standby
* Mode_Retract_To_Ready
* Mode_Normal
* Mode_NoInit_To_Ready
* ERROR
*/
//Are we done retracting
if (
positionLive.SystemStatus.RetractStatus == 1 ||
positionLive.SystemStatus.RetractStatus == 3)
{
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
else
{
//Look for timeout
if ((DateTime.Now - m_LastCmd) > m_RetractDelay)
{
System.Console.WriteLine("Retract delay expired");
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
}
}
catch (Exception e)
{
System.Console.WriteLine(
"JACO API diagnostic API Failed : ");
System.Console.WriteLine(e.ToString());
m_IsRetracting = false;
// Stop the retract/reset command.
m_Cmd.ButtonValue[2] = 0;
m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
m_LastCmd = DateTime.Now;
}
return;
}
// 200 ms watchdog, reset the command if it's too old.
if ((DateTime.Now - m_LastCmd) > m_WatchDogDelay)
{
m_Cmd = new CJoystickValue();
}
// NOTE: Disabled for now, testing trajectory mode.
//m_Arm.ControlManager.SendJoystickFunctionnality(m_Cmd);
}
static void Main (string[] args)
{
bool dll = args.Length > 0 && String.Compare(args[0], "dll") == 0;
if (dll) {
_error = "#ERROR [JACO]: ";
_info = "#INFO [JACO]: ";
_warning = "#WARNING [JACO]: ";
}
try {
_jaco = new CJacoArm(Crypto.GetInstance().Encrypt("C6H12O6h2so4"));
Console.WriteLine(_info + "Successfully connected to Jaco.");
} catch (CAccessDeniedException) {
Console.WriteLine(_error + "Access to Jaco denied. Please check password and USB connection.");
return;
} catch (Exception) {
Console.WriteLine(_error + "Could not connect to Jaco.");
return;
}
_cmd = new CJoystickValue();
_zones = _jaco.ConfigurationsManager.GetProtectionZones();
_jaco.ControlManager.StartControlAPI();
_api = true;
_data = new float[37];
for (int i=0; i<37; i++) _data[i] = 0f;
if (dll) {
Console.WriteLine(_info + "Active protection zones: " + _zones.NbZoneActive);
return;
}
TerminalCommand(args);
Deinit();
}
