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