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