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