public void SendJointsAngleRads(double j0, double j1, double j2, double j3, double j4, double j5)
{
if (m_Arm.JacoIsReady())
{
try
{
CPointsTrajectory pointsTrajectory = new CPointsTrajectory();
CVectorAngle vector = new CVectorAngle();
//Change into degrees
j0 *= (180.0 / Math.PI);
j1 *= (180.0 / Math.PI);
j2 *= (180.0 / Math.PI);
j3 *= (180.0 / Math.PI);
j4 *= (180.0 / Math.PI);
j5 *= (180.0 / Math.PI);
//Angle conversion for Jaco according to DH model
vector.Angle = new Single[6] {
Convert.ToSingle(-j0 + 180),
Convert.ToSingle(j1 + 270),
Convert.ToSingle(-j2 + 90),
Convert.ToSingle(-j3 + 180),
Convert.ToSingle(-j4 + 180),
Convert.ToSingle(-j5 + 260)
};
//This is weird, do we have to set this?
vector.NbAngle = 6;
//Send trajectory
CTrajectoryInfo trajectory = new CTrajectoryInfo();
trajectory.UserPosition.AnglesJoints = vector;
trajectory.UserPosition.PositionType = CJacoStructures.PositionType.AngularPosition;
//dont move Hand
trajectory.UserPosition.HandMode = CJacoStructures.HandMode.NoMovements;
pointsTrajectory.Add(trajectory);
m_Arm.ControlManager.SendTrajectoryFunctionnality(pointsTrajectory);
}
catch (Exception e)
{
System.Console.WriteLine(
"JACO API SendTrajectory Failed (Angular Joint Positions): ");
System.Console.WriteLine(e.ToString());
}
}
}
//must call before every frame
public void resetPoint()
{
try
{
if (Jaco.JacoIsReady())
{
CVectorAngle JointPosition = Jaco.ConfigurationsManager.GetJointPositions();
pointTraj.UserPosition.AnglesJoints = JointPosition;
}
else
{
Logger.addLog("Jaco not ready - unable to get joint positions", true);
}
}
catch (Exception)
{
Logger.addLog("Error in Jaco - unable to get joint positions", true);
}
}
public void SendJointsAngleRads(double j0, double j1, double j2, double j3, double j4, double j5)
{
if (m_Arm.JacoIsReady())
{
try
{
CPointsTrajectory pointsTrajectory = new CPointsTrajectory();
CVectorAngle vector = new CVectorAngle();
//Change into degrees
j0 *= (180.0 / Math.PI);
j1 *= (180.0 / Math.PI);
j2 *= (180.0 / Math.PI);
j3 *= (180.0 / Math.PI);
j4 *= (180.0 / Math.PI);
j5 *= (180.0 / Math.PI);
//Angle conversion for Jaco according to DH model
vector.Angle = new Single[6] {
Convert.ToSingle(-j0 + 180),
Convert.ToSingle( j1 + 270),
Convert.ToSingle(-j2 + 90 ),
Convert.ToSingle(-j3 + 180),
Convert.ToSingle(-j4 + 180),
Convert.ToSingle(-j5 + 260)};
//This is weird, do we have to set this?
vector.NbAngle = 6;
//Send trajectory
CTrajectoryInfo trajectory = new CTrajectoryInfo();
trajectory.UserPosition.AnglesJoints = vector;
trajectory.UserPosition.PositionType = CJacoStructures.PositionType.AngularPosition;
pointsTrajectory.Add(trajectory);
m_Arm.ControlManager.SendTrajectoryFunctionnality(pointsTrajectory);
}
catch (Exception e)
{
System.Console.WriteLine(
"JACO API SendTrajectory Failed (Angular Joint Positions): ");
System.Console.WriteLine(e.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 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);
}
