예제 #1
0
        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());
                }
            }
        }
예제 #2
0
        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);
        }
예제 #3
0
        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());
                }
            }
        }
예제 #4
0
        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");
     }
 }
예제 #6
0
        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());
                }
            }
        }
예제 #7
0
        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());
                }
            }
        }
예제 #8
0
        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();
 }
예제 #10
0
  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;
  }