public void SendFingersAngleRads(double f1, double f2, double f3) { if (m_Arm.JacoIsReady()) { try { CPointsTrajectory pointsTrajectory = new CPointsTrajectory(); //CVectorAngle vector = new CVectorAngle(); CTrajectoryInfo trajectory = new CTrajectoryInfo(); //trajectory.UserPosition.AnglesJoints = vector; trajectory.UserPosition.PositionType = CJacoStructures.PositionType.PositionNoMovements; trajectory.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode; //Trajectory needs to be converted to degrees trajectory.UserPosition.FingerPosition[0] = Convert.ToSingle(f1 * 180.0 / Math.PI); trajectory.UserPosition.FingerPosition[1] = Convert.ToSingle(f2 * 180.0 / Math.PI); trajectory.UserPosition.FingerPosition[2] = Convert.ToSingle(f3 * 180.0 / Math.PI); pointsTrajectory.Add(trajectory); m_Arm.ControlManager.SendTrajectoryFunctionnality(pointsTrajectory); } catch (Exception e) { System.Console.WriteLine( "JACO API SendTrajectory Failed (Finger Positions): "); System.Console.WriteLine(e.ToString()); } } }
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 SetTargetPosition(CTrajectoryInfo point, bool cartesian, float x1, float x2, float x3, float x4, float x5, float x6, float f1, float f2, float f3) { if (cartesian) { point.UserPosition.Position = new CVectorEuler(); point.UserPosition.Position.Position[CVectorEuler.COORDINATE_X] = x1; point.UserPosition.Position.Position[CVectorEuler.COORDINATE_Y] = x2; point.UserPosition.Position.Position[CVectorEuler.COORDINATE_Z] = x3; point.UserPosition.Position.Rotation[CVectorEuler.THETA_X] = x4; point.UserPosition.Position.Rotation[CVectorEuler.THETA_Y] = x5; point.UserPosition.Position.Rotation[CVectorEuler.THETA_Z] = x6; point.UserPosition.PositionType = CJacoStructures.PositionType.CartesianPosition; } else { point.UserPosition.AnglesJoints = new CVectorAngle(); point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_1] = x1; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_2] = x2; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_3] = x3; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_4] = x4; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_5] = x5; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_6] = x6; point.UserPosition.PositionType = CJacoStructures.PositionType.AngularPosition; } point.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode; point.UserPosition.FingerPosition[0] = f1; point.UserPosition.FingerPosition[1] = f2; point.UserPosition.FingerPosition[2] = f3; }
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()); } } }
static void SetTargetSpeed(CTrajectoryInfo point, float linear, float angular, float finger) { if (linear >= 0f && linear <= 0.15f) { point.ZoneLimitation.LinearSpeed = linear; Console.WriteLine(_info + "Linear speed set to " + linear); point.LimitationActive = true; } if (angular >= 0f && angular <= 0.6f) { point.ZoneLimitation.AngularSpeed = angular; point.LimitationActive = true; } if (finger >= 0f && finger <= 0.15f) { point.ZoneLimitation.FingersSpeed = finger; point.LimitationActive = true; } }
static bool SetPosition(bool cartesian, float x1, float x2, float x3, float x4, float x5, float x6, float f1, float f2, float f3) { if (!CheckControl()) { return(false); } CheckMode(cartesian); _cartesian = cartesian; CTrajectoryInfo _point = new CTrajectoryInfo(); SetTargetPosition(_point, cartesian, x1, x2, x3, x4, x5, x6, f1, f2, f3); CPointsTrajectory trajectory = new CPointsTrajectory(); trajectory.Add(_point); _jaco.ControlManager.SendTrajectoryFunctionnality(trajectory); return(true); }
public CJacoArm init() { const string MyValidPassword = "******"; pointTraj = new CTrajectoryInfo(); try { Jaco = new CJacoArm(Crypto.GetInstance().Encrypt(MyValidPassword)); Jaco.ControlManager.StartControlAPI(); } catch (Exception ex) { Logger.addLog("Error initializing Jaco!\n" + ex.Message, true); return(Jaco); } if (Jaco.JacoIsReady()) { CClientConfigurations config = new CClientConfigurations(); config.ClientName = "Arup"; config.MaxAngularSpeed = 0.2f; config.Organization = "SV"; Jaco.ConfigurationsManager.SetClientConfigurations(config); Jaco.ControlManager.EraseTrajectories(); Logger.addLog("Jaco initialized and ready"); return(Jaco); } else { Logger.addLog("Jaco initialized but not ready", true); return(Jaco); } }
public void Delay(CTrajectoryInfo point, CJacoArm jArm) { float J1, J2, J3 /*, J4, J5, J6, F1, F2, F3*/; int k = 0; int jacoTimeout = 0; //max delay timeout for jaco to track arm CAngularInfo info; float thresh = 6; do { try { info = jArm.ControlManager.GetPositioningAngularInfo(); } catch (Exception) { Logger.addLog("Error in obtaining Jaco current angles", true); return; } J1 = Math.Abs(info.Joint1 - point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_1]); J2 = Math.Abs(info.Joint2 - point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_2]); J3 = Math.Abs(info.Joint3 - point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_3]); // J4 = Math.Abs(info.Joint4 - point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_4]); // J5 = Math.Abs(info.Joint5 - point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_5]); // J6 = Math.Abs(info.Joint6 - point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_6]); // F1 = Math.Abs(info.Finger1 - point.UserPosition.FingerPosition[0]); // F2 = Math.Abs(info.Finger2 - point.UserPosition.FingerPosition[1]); // F3 = Math.Abs(info.Finger3 - point.UserPosition.FingerPosition[2]); k++; }while (k < jacoTimeout && !((J1 < thresh) && (J2 < thresh) && (J3 < thresh) /* && (J4 < 2.00)) && (J5 < 2.00)) && (J6 < 2.00)*/)); if (k >= jacoTimeout) { Logger.addLog("Timeout of " + jacoTimeout.ToString() + " loops exceeded"); } }
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); }
private CTrajectoryInfo GenerateFingerTrajectory(float finger_1, float finger_2, float finger_3) { CTrajectoryInfo fingerTrajectory = new CTrajectoryInfo(); fingerTrajectory.LimitationActive = false; fingerTrajectory.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode; fingerTrajectory.UserPosition.PositionType = CJacoStructures.PositionType.AngularPosition; fingerTrajectory.UserPosition.AnglesJoints = m_Arm.ConfigurationsManager.GetJointPositions(); fingerTrajectory.UserPosition.FingerPosition[0] = finger_1; fingerTrajectory.UserPosition.FingerPosition[1] = finger_2; fingerTrajectory.UserPosition.FingerPosition[2] = finger_3; return fingerTrajectory; }
private CTrajectoryInfo GeneratePoseTrajectory(float []pose) { CTrajectoryInfo poseTrajectory = new CTrajectoryInfo(); poseTrajectory.LimitationActive = false; poseTrajectory.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode; poseTrajectory.UserPosition.PositionType = CJacoStructures.PositionType.CartesianPosition; poseTrajectory.UserPosition.Position.Position[CVectorEuler.COORDINATE_X] = pose[0]; poseTrajectory.UserPosition.Position.Position[CVectorEuler.COORDINATE_Y] = pose[1]; poseTrajectory.UserPosition.Position.Position[CVectorEuler.COORDINATE_Z] = pose[2]; poseTrajectory.UserPosition.Position.Rotation[CVectorEuler.THETA_X] = pose[3]; poseTrajectory.UserPosition.Position.Rotation[CVectorEuler.THETA_Y] = pose[4]; poseTrajectory.UserPosition.Position.Rotation[CVectorEuler.THETA_Z] = pose[5]; return poseTrajectory; }
private CTrajectoryInfo GenerateJointTrajectory(float []jointAngles) { CTrajectoryInfo jointTrajectory = new CTrajectoryInfo(); jointTrajectory.LimitationActive = false; jointTrajectory.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode; jointTrajectory.UserPosition.PositionType = CJacoStructures.PositionType.AngularPosition; jointTrajectory.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_1] = jointAngles[0]; jointTrajectory.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_2] = jointAngles[1]; jointTrajectory.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_3] = jointAngles[2]; jointTrajectory.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_4] = jointAngles[3]; jointTrajectory.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_5] = jointAngles[4]; jointTrajectory.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_6] = jointAngles[5]; jointTrajectory.UserPosition.FingerPosition[0] = joint_info.Finger1; jointTrajectory.UserPosition.FingerPosition[1] = joint_info.Finger2; jointTrajectory.UserPosition.FingerPosition[2] = joint_info.Finger3; return jointTrajectory; }
static bool SetPositionWithSpeed (bool cartesian, float x1, float x2, float x3, float x4, float x5, float x6, float f1, float f2, float f3, float sc, float sa, float sf) { if (!CheckControl()) return false; CheckMode(cartesian); _cartesian = cartesian; CTrajectoryInfo _point = new CTrajectoryInfo(); SetTargetPosition(_point, cartesian, x1, x2, x3, x4, x5, x6, f1, f2, f3); SetTargetSpeed(_point, sc, sa, sf); CPointsTrajectory trajectory = new CPointsTrajectory(); trajectory.Add(_point); _jaco.ControlManager.SendTrajectoryFunctionnality(trajectory); return true; }
static void SetTargetSpeed (CTrajectoryInfo point, float linear, float angular, float finger) { if (linear >= 0f && linear <= 0.15f) { point.ZoneLimitation.LinearSpeed = linear; Console.WriteLine(_info + "Linear speed set to " + linear); point.LimitationActive = true; } if (angular >= 0f && angular <= 0.6f) { point.ZoneLimitation.AngularSpeed = angular; point.LimitationActive = true; } if (finger >= 0f && finger <= 0.15f) { point.ZoneLimitation.FingersSpeed = finger; point.LimitationActive = 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()); } } }
static void SetTargetPosition (CTrajectoryInfo point, bool cartesian, float x1, float x2, float x3, float x4, float x5, float x6, float f1, float f2, float f3) { if (cartesian) { point.UserPosition.Position = new CVectorEuler(); point.UserPosition.Position.Position[CVectorEuler.COORDINATE_X] = x1; point.UserPosition.Position.Position[CVectorEuler.COORDINATE_Y] = x2; point.UserPosition.Position.Position[CVectorEuler.COORDINATE_Z] = x3; point.UserPosition.Position.Rotation[CVectorEuler.THETA_X] = x4; point.UserPosition.Position.Rotation[CVectorEuler.THETA_Y] = x5; point.UserPosition.Position.Rotation[CVectorEuler.THETA_Z] = x6; point.UserPosition.PositionType = CJacoStructures.PositionType.CartesianPosition; } else { point.UserPosition.AnglesJoints = new CVectorAngle(); point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_1] = x1; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_2] = x2; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_3] = x3; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_4] = x4; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_5] = x5; point.UserPosition.AnglesJoints.Angle[CVectorAngle.JOINT_6] = x6; point.UserPosition.PositionType = CJacoStructures.PositionType.AngularPosition; } point.UserPosition.HandMode = CJacoStructures.HandMode.PositionMode; point.UserPosition.FingerPosition[0] = f1; point.UserPosition.FingerPosition[1] = f2; point.UserPosition.FingerPosition[2] = f3; }