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);
}
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 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 string sendMultiTraj(CPointsTrajectory traj)
{
try
{
if (Jaco.JacoIsReady())
{
Jaco.ControlManager.SendBasicTrajectory(traj);
Logger.addLog("Trajectory Sent");
return("Angle set");
}
else
{
Logger.addLog("Jaco not ready - unable to send trajectory", true);
return("Jaco not ready - unable to send trajectory");
}
}
catch (Exception)
{
Logger.addLog("Error in updating trajectory", true);
return("Error in updating trajectory");
}
}
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());
}
}
}
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);
}
//used to clear existing trajectory
void TrajClear()
{
m_PointsTrajectory = new CPointsTrajectory();
}
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;
}
