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);
}
public void JacoRefreshEncoder()
{
try
{
//if (m_Arm.JacoIsReady())
// {
joint_info = m_Arm.ControlManager.GetPositioningAngularInfo();
current_info = m_Arm.ControlManager.GetCurrentAngularInfo();
pose_info = m_Arm.ControlManager.GetCommandCartesianInfo();
trajectory_info = m_Arm.ControlManager.GetInfoFIFOTrajectory();
positionLive = m_Arm.DiagnosticManager.DataManager.GetPositionLogLiveFromJaco();
//button states
m_State.power_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[0]);
m_State.retract_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[1]);
m_State.one_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[2]);
m_State.two_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[3]);
m_State.three_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[4]);
m_State.left_joystick_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[5]);
m_State.right_joystick_button = Convert.ToBoolean(positionLive.JoystickValue.ButtonValue[6]);
//joystick states
m_State.forward_backward = positionLive.JoystickValue.InclineFB;
m_State.left_right = positionLive.JoystickValue.InclineLR;
m_State.rotate = positionLive.JoystickValue.Rotate;
// Based on Observation of actual model
// getting joints angles
// Normailising the joint angle (-180 to +180)
m_State.shoulder_yaw.angle = Normalize( ((joint_info.Joint1) - 180.0) * Constants.DTR );
m_State.shoulder_pitch.angle = Normalize( ((joint_info.Joint2) - 270.0) * Constants.DTR );
m_State.elbow_pitch.angle = Normalize( ((joint_info.Joint3) - 90.0 ) * Constants.DTR );
m_State.elbow_roll.angle = Normalize( ((joint_info.Joint4) - 180.0) * Constants.DTR );
m_State.wrist_roll.angle = Normalize( ((joint_info.Joint5) - 180.0) * Constants.DTR );
m_State.hand_roll.angle = Normalize( ((joint_info.Joint6) - 260.0) * Constants.DTR );
// based on DH model provided by Kinova
/*m_State.shoulder_yaw.angle = (180.0 - (joint_info.Joint1)) * Constants.DTR;
m_State.shoulder_pitch.angle = ((joint_info.Joint2) - 270.0) * Constants.DTR;
m_State.elbow_pitch.angle = (90.0 - (joint_info.Joint3)) * Constants.DTR;
m_State.elbow_roll.angle = (180.0 - (joint_info.Joint4)) * Constants.DTR;
m_State.wrist_roll.angle = (180.0 - (joint_info.Joint5)) * Constants.DTR;
m_State.hand_roll.angle = (260.0 - (joint_info.Joint6)) * Constants.DTR;*/
// getting joint current
current_info = m_Arm.ControlManager.GetCurrentAngularInfo();
m_State.current_joint_1 = current_info.Joint1;
m_State.current_joint_2 = current_info.Joint2;
m_State.current_joint_3 = current_info.Joint3;
m_State.current_joint_4 = current_info.Joint4;
m_State.current_joint_5 = current_info.Joint5;
m_State.current_joint_6 = current_info.Joint6;
// getting fingers angles
m_State.finger_1.angle = (joint_info.Finger1) * Constants.DTR;
m_State.finger_2.angle = (joint_info.Finger2) * Constants.DTR;
m_State.finger_3.angle = (joint_info.Finger3) * Constants.DTR;
// getting finger current
m_State.current_finger_1 = current_info.Finger1;
m_State.current_finger_2 = current_info.Finger2;
m_State.current_finger_3 = current_info.Finger3;
// getting the pose
// !!! CAUTION !!!!
// Jaco arm API is calculating the forward kinematics not fast enough or its not
// calculating at all...
//
// todo: need to replace by jaco_arm_kinematics FK
m_State.hand_position_x = pose_info.X ;
m_State.hand_position_y = pose_info.Y ;
m_State.hand_position_z = pose_info.Z ;
m_State.hand_orientation_x = pose_info.ThetaX ;
m_State.hand_orientation_y = pose_info.ThetaY ;
m_State.hand_orientation_z = pose_info.ThetaZ ;
// getting the trajectory info
m_State.current_traj = trajectory_info.StillInFIFO;
//}
}
catch (Exception ex)
{
System.Console.WriteLine("EXCEPTION in JacoRefreshEncoder");
System.Console.WriteLine(ex.ToString());
}
}
static void Test()
{
///< \todo time
CJacoGripper g = _jaco.ConfigurationsManager.GetJacoGripperInfo();
CJacoFinger f1 = g.Fingers[CJacoGripper.FINGER_1];
Console.WriteLine("Pos: " + f1.ActualPosition);
Console.WriteLine("Current: " + f1.ActualCurrent);
Console.WriteLine("AvgCur: " + f1.ActualAverageCurrent);
Console.WriteLine("Force: " + f1.ActualForce);
Console.WriteLine("Force: " + f1.MaxForce);
Console.WriteLine("Speed: " + f1.ActualSpeed);
Console.WriteLine("Accel: " + f1.ActualAcceleration);
Console.WriteLine("Temp: " + f1.ActualTemperature);
Console.WriteLine();
CCartesianInfo info = _jaco.ControlManager.GetForceCartesianInfo();
System.Console.WriteLine(" Force X : " + info.X);
System.Console.WriteLine(" Force Y : " + info.Y);
System.Console.WriteLine(" Force Z : " + info.Z);
System.Console.WriteLine(" Force Theta X : " + info.ThetaX);
System.Console.WriteLine(" Force Theta Y : " + info.ThetaY);
System.Console.WriteLine(" Force Theta Z : " + info.ThetaZ);
System.Console.WriteLine(" Force Finger #1 : " + info.Finger1);
System.Console.WriteLine(" Force Finger #2 : " + info.Finger2);
System.Console.WriteLine(" Force Finger #3 : " + info.Finger3);
Console.WriteLine();
CAngularInfo ang = _jaco.ControlManager.GetCurrentAngularInfo();
System.Console.WriteLine(" Current Joint 1 : " + ang.Joint1);
System.Console.WriteLine(" Current Joint 2 : " + ang.Joint2);
System.Console.WriteLine(" Current Joint 3 : " + ang.Joint3);
System.Console.WriteLine(" Current Joint 4 : " + ang.Joint4);
System.Console.WriteLine(" Current Joint 5 : " + ang.Joint5);
System.Console.WriteLine(" Current Joint 6 : " + ang.Joint6);
System.Console.WriteLine(" Current Finger #1 : " + ang.Finger1);
System.Console.WriteLine(" Current Finger #2 : " + ang.Finger2);
System.Console.WriteLine(" Current Finger #3 : " + ang.Finger3);
Console.WriteLine();
ang = _jaco.ControlManager.GetForceAngularInfo();
System.Console.WriteLine(" Force Joint 1 : " + ang.Joint1);
System.Console.WriteLine(" Force Joint 2 : " + ang.Joint2);
System.Console.WriteLine(" Force Joint 3 : " + ang.Joint3);
System.Console.WriteLine(" Force Joint 4 : " + ang.Joint4);
System.Console.WriteLine(" Force Joint 5 : " + ang.Joint5);
System.Console.WriteLine(" Force Joint 6 : " + ang.Joint6);
System.Console.WriteLine(" Force Finger #1 : " + ang.Finger1);
System.Console.WriteLine(" Force Finger #2 : " + ang.Finger2);
System.Console.WriteLine(" Force Finger #3 : " + ang.Finger3);
Console.WriteLine();
GetData();
for (int i = 0; i < 37; i++)
{
Console.Write(_data[i] + " ");
}
Console.WriteLine();
System.Console.WriteLine("Max Zones: " + CZoneList.NB_ZONE_MAX);
}
