public void GoToPose(JacoPose pose)
{
CPointsTrajectory traj = new CPointsTrajectory();
CTrajectoryInfo ti = new CTrajectoryInfo();
ti.UserPosition.PositionType =
CJacoStructures.PositionType.CartesianPosition;
//ti.UserPosition.PositionType =
//CJacoStructures.PositionType.AngularPosition;
ti.UserPosition.HandMode =
CJacoStructures.HandMode.NoMovements;
CVectorEuler p = new CVectorEuler();
p.Position = new Single[3] {
Convert.ToSingle(pose.x),
Convert.ToSingle(pose.y),
Convert.ToSingle(pose.z)
};
p.Rotation = new Single[3] {
Convert.ToSingle(pose.e_x),
Convert.ToSingle(pose.e_y),
Convert.ToSingle(pose.e_z)
};
ti.UserPosition.Position = p;
traj.Add(ti);
m_Arm.ControlManager.SendTrajectoryFunctionnality(traj);
}
static void PrintZones()
{
int znum = _zones.NbZoneActive;
Console.WriteLine(_info + "Active Zones: " + znum);
for (int z = 0; z < znum; z++)
{
Console.WriteLine(_info + "Zone " + z + " (" + _zones.Zones[z].ZoneShape.ShapeType + ")");
for (int p = 0; p < 4; p++)
{
CVectorEuler point = _zones.Zones[z].ZoneShape.Point[p];
Console.WriteLine(_info + " Base " + p + ": " + point.Position[0] + " " + point.Position[1] + " " + point.Position[2]);
}
Console.WriteLine(_info + " Height: " + _zones.Zones[z].ZoneShape.Point[4].Position[2]);
Console.WriteLine(_info + " Angular Limit: " + _zones.Zones[z].ZoneLimitation.AngularSpeed);
Console.WriteLine(_info + " Linear Limit: " + _zones.Zones[z].ZoneLimitation.LinearSpeed);
}
}
static bool SaveZonesToFile(string filename)
{
int num = _zones.NbZoneActive;
StreamWriter file = new StreamWriter(filename);
for (int z = 0; z < num; z++)
{
file.WriteLine(_info + "# Zone " + z);
for (int p = 0; p < 4; p++)
{
CVectorEuler point = _zones.Zones[z].ZoneShape.Point[p];
file.WriteLine(point.Position[0] + " " + point.Position[1] + " " + point.Position[2]);
}
file.WriteLine(_zones.Zones[z].ZoneLimitation.AngularSpeed + " " + _zones.Zones[z].ZoneLimitation.LinearSpeed + " " + _zones.Zones[z].ZoneShape.Point[4].Position[2]);
}
file.Close();
Console.WriteLine(_info + "Wrote zone configuration to file \"" + filename + "\".");
return(true);
}
public void GoToPose(JacoPose pose)
{
CPointsTrajectory traj = new CPointsTrajectory();
CTrajectoryInfo ti= new CTrajectoryInfo();
ti.UserPosition.PositionType =
CJacoStructures.PositionType.CartesianPosition;
//ti.UserPosition.PositionType =
//CJacoStructures.PositionType.AngularPosition;
ti.UserPosition.HandMode =
CJacoStructures.HandMode.NoMovements;
CVectorEuler p = new CVectorEuler();
p.Position = new Single[3] {
Convert.ToSingle(pose.x),
Convert.ToSingle(pose.y),
Convert.ToSingle(pose.z)};
p.Rotation = new Single[3] {
Convert.ToSingle(pose.e_x),
Convert.ToSingle(pose.e_y),
Convert.ToSingle(pose.e_z)};
ti.UserPosition.Position = p;
traj.Add(ti);
m_Arm.ControlManager.SendTrajectoryFunctionnality(traj);
}
/// \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 float[] GetData()
{
CVectorEuler cpos = _jaco.ConfigurationsManager.GetHandPosition();
CVectorAngle apos = _jaco.ConfigurationsManager.GetJointPositions();
CCartesianInfo fpos = _jaco.ControlManager.GetPositioningCartesianInfo();
CCartesianInfo cforce = _jaco.ControlManager.GetForceCartesianInfo();
CAngularInfo aforce = _jaco.ControlManager.GetForceAngularInfo();
CAngularInfo curr = _jaco.ControlManager.GetCurrentAngularInfo();
// cartesian position
_data[0] = cpos.Position[CVectorEuler.COORDINATE_X];
_data[1] = cpos.Position[CVectorEuler.COORDINATE_Y];
_data[2] = cpos.Position[CVectorEuler.COORDINATE_Z];
_data[3] = cpos.Rotation[CVectorEuler.THETA_X];
_data[4] = cpos.Rotation[CVectorEuler.THETA_Y];
_data[5] = cpos.Rotation[CVectorEuler.THETA_Z];
// angular position
_data[6] = apos.Angle[CVectorAngle.JOINT_1];
_data[7] = apos.Angle[CVectorAngle.JOINT_2];
_data[8] = apos.Angle[CVectorAngle.JOINT_3];
_data[9] = apos.Angle[CVectorAngle.JOINT_4];
_data[10] = apos.Angle[CVectorAngle.JOINT_5];
_data[11] = apos.Angle[CVectorAngle.JOINT_6];
// fingers
_data[12] = fpos.Finger1;
_data[13] = fpos.Finger2;
_data[14] = fpos.Finger3;
// API control
_data[15] = _jaco.ControlManager.IsApiInControl() ? 1f : 0f;
// cartesian force
_data[16] = cforce.X;
_data[17] = cforce.Y;
_data[18] = cforce.Z;
_data[19] = cforce.ThetaX;
_data[20] = cforce.ThetaY;
_data[21] = cforce.ThetaZ;
// angular force
_data[22] = aforce.Joint1;
_data[23] = aforce.Joint2;
_data[24] = aforce.Joint3;
_data[25] = aforce.Joint4;
_data[26] = aforce.Joint5;
_data[27] = aforce.Joint6;
// motor currents
_data[28] = curr.Joint1;
_data[29] = curr.Joint2;
_data[30] = curr.Joint3;
_data[31] = curr.Joint4;
_data[32] = curr.Joint5;
_data[33] = curr.Joint6;
_data[34] = curr.Finger1;
_data[35] = curr.Finger2;
_data[36] = curr.Finger3;
return(_data);
}
