Edit on GitHub

Examples

ArmMonitor

ArmMonitor allows viewing of a joint current position, target position, end effector’s pose, and joint parameters (Velocity, Acceleration, Position P, I, D Gain, Velocity P, I Gain). Change the values from the table below to see changes.

How to Use ArmMonitor

ArmMonitor01

You will need to enter the COM port number and baud rate. Simply enter the values and “Succeed to open USB2DYNAMIXEL” should appear onscreen followed by “Press any key to move first pose.” Use the keyboard to move the arm.

The following table is a list of baud rate values and its corresponding speed; Robotis Manipulator default value is 3 (1Mbps).

The following table is a list of baud rate values and its corresponding speed; Robotis Manipulator default value is 3 (1Mbps).

Baudrate Number baudrate
0 2,400 bps
1 57,600 bps
2 115,200 bps
3 1,000,000 bps
4 2,000,000 bps
5 3,000,000 bps

The photo below is the arm in its “arrival” pose.

The Goal Value of Arm의 Calc<rad> value (enclosed by the red frame) can be increased with the‘]’ key. The unit is (π/180)rad.
After adjusting the joint, check if the Manipulator follows.

ArmMonitor02

ArmMonitor02 allows direct control of the end effector. Control the end effector is done by ComputeIK function where it moves each joint to its solution position (rad).

To setup and run ArmMonitor02 follow the same procedure as in ArmMonitor01.
As in ArmMonitor01 you will be asked to enter COM port number and baud rate. You should also see “Succeed to open USB2DYNAMIXEL” followed by “Press any key to move first pose.” The arm moves to its initial pose.

The photo below is the arm in its “arrival” pose.

The different values of the end effector depicted from the red areas with “1” and “2” (from the screen output image above) is due to the difference of DYNAMIXEL Pro’s Goal Position and Present Position values (gear backlash) and DH with the point of origin. ”1” shows the end effector’s pose via calculations from kinematics and “2” the actual pose.

Press the ] key to increase the end effector’s pose value by (π/180)rad ; X increases by 2mm. Visually verify arm movement every time when changing position.

Arm Monitor Source Description

cmd_process.cpp

These 4 functions allows the directional keys to control cursor location.

Simple P to P

WARNING : Product may move fast with this example. When testing this example keep a safe distance while able to cut power off in case of undesired operation.

Simple P to P is the end effector’s move point (from P1 to P2).

How to Use Simple P to P

To start Simple P to P follow the same procedure for ArmMonitor. Then press the Ctrl + F5 keys to run.

You will be asked for COM port number and baud rate.
If succeeded then you will see a ‘Succeed to open USB2DYNAMIXEL’ followed by ‘Press any key to move first pose.’ Press a key to move the arm to its initial pose.
Then press a key to begin P2P Motion. The photo below is the arm in its initial pose.

Simple P to P displays the joints’ pose(rad). In Simple P to P prssing the ‘p’ or ‘P’ will cause motion to pause. Press the ESC key to end.

Simple P to P Source Description

vecd P1, P2;
P1.resize(RobotisArm.GetRobotInfo()->size());
P2.resize(RobotisArm.GetRobotInfo()->size());

P1, P2 sets every joint’s position.

P1.fill(0.0);
P1 -= gvdAngleGapCalcandDynamixelRad;
P2.fill(0.5);
P2 -= gvdAngleGapCalcandDynamixelRad;

P1.fill, P2.fill input every joint’s position(rad) individually. Differences between DH Configuration’s point of origin and actual point of origin are taken into consideration so P1 and P2 are to be adjusted accordingly.

ArmComm.Arm_Set_Position_PID_Gain(64, 0, 0);

Joint’s Position P, I, and D gain values.respectively.

ArmComm.Arm_Set_JointVelocity(0);

Joint’s velocity value .0 denotes max velocity.

ArmComm.Arm_Set_JointAcceleration(0);

Joint’s acceleration value .0 denotes max velocity.

ArmTrajectory.ClearMF();

MotionProfile clears the set space..

ArmTrajectory.Set_P2P(P1, P2, 10.0, 0.5);

Sets P1, P2(Start, EndPose). In this case P1 is 0.0 rad and P2 is 0.5 rad. Trajectory is from P1 to P2

ArmTrajectory.Set_P2P(P2, P1, 10.0, 0.5);

Sets P1, P2(Start, EndPose). In this case P1 is 0.0 rad and P2 is 0.5 rad. Trajectory is from P2 to P1

MotionPlayer.All_Info_Reload();

MotionProfile calls Info(Robot, Kinematics, Trajectory).

MoionPlayer.Initialize();

MotionProfile, Step, are initialized.

MotionPlayer.Set_Time_Period(DEFAULT_Ctrl_TIME_PERIOD);

Sets time period. For value lesser than 0 then a default value (=8) gets inputted.

Simple IK

WARNING : Use of this example may pose safety risks. When testing the example keep a safe distance while able to cut power off in case of undesired operation.

Control EndEffector Position -> +5mm
orientation -> +(3π/180)rad		 Position -> -5mm
orientation -> -(3π/180)rad
Position X q a
Position Y w s
Position Z e d
Orientation Roll r f
Orientation Pitch t g
Orientation Yaw y h

How to Use Simple IK

To start Simple IK start a new project just like SimplePtoP. Then press the Ctrl + F5 ekys to begin.

In Simple IK you will be asked for COM port and baud rate numbers. If succeeded you will see a ‘Succeed to open USB2DYNAMIXEL’ followed by ‘Press any key to move first pose.’ Press a key to begin. The arm moves to its initial pose as shown below.

This windows pops up after the arms moves to its initial pose. The values printed are the joints’ angles(rad). Press the keys(ex : q, w….) to move the end effector.

Simple IK q key control the 3rd value.
Q controls the end effector position (X) by increasing delta(5mm)amounts.
Visually verify arm movement every time when changing position.

Press the q and r keys 3 times each. The r key controls the end effector’s roll. The orientation (Roll, Pitch, Yaw) change by (3π/180)rad per keystroke.
Visually verify arm movement every time when changing position.

Simple IK Source Description

if(temp == 'q')
{
  DesiredPose = CurrentPose;
  DesiredPose.x += delta;
  ArmKinematics.ComputeIK(DesiredPose, &angle_rad, angle_rad, &ErrorStatus);
  if(ErrorStatus == ARMSDK_NO_ERROR)
  {
    cout<<"Answer"<<endl;
    cout<<angle_rad<<endl<<endl;
    ArmComm.Arm_Set_JointPosition(RobotisArm.Rad2Value(angle_rad + gvdAngleGapCalcandDynamixelRad));
  }
  else if(ErrorStatus & ARMSDK_ACCEPTABLE_ERROR)
  {
    cout<< "No IK solution" <<endl;
    cout<< "But the calcuation result is acceptable" <<endl;
    char answer;
    while(true)
    {
      cout<< "Do you want make the Robot move? (Y/N)";
      cin >> answer;
      if((answer == 'y') || (answer == 'n') || (answer == 'Y') || (answer == 'N'))
        break;
      else
        cout<< "Invaild Answer"<<endl;
    }
    if((answer == 'y') || (answer == 'Y') )
      ArmComm.Arm_Set_JointPosition(RobotisArm.Rad2Value(angle_rad + gvdAngleGapCalcandDynamixelRad));
    else
      continue;
  }
  else
  {
    cout<< "No IK Solution" <<endl;
    continue;
  }
  ArmKinematics.Forward(angle_rad, &CurrentPose);
}

The code shows that by pressing the q key the program runs. A press of q moves the end effector pose in the (X) coordinate by delta (5mm).
If there are no errors the end effector will move according to keystroke. All joints are in radians. Press the ‘q‘ key to to goal pose by X position in delta incrememts.

Despite having errors and not being able to get the IK moving can be allowed. If ‘Do you want make the Robot move? (Y/N)’ appears onscreen press the y key to move the end effector in the X coordinate by +5mm. Then the joints pose(rad) are displayed.

WARNING : Product may go to pose fast after pressing the Y key posing a safety risk. When testing the example keep a safe distance while able to cut power off in case of undesired operation.

When error is too large and IK is unrealizable ‘No IK Solution‘ will be displayed the end effector will remain as is. The sample code from above is broken down below.

ArmKinematics.ComputeIK(DesiredPose, &angle_rad, angle_rad, &ErrorStatus);

All joints set to a desired pose by taking input from DesiredPose and angle_rad. Once DesiredPose values go to CurrentPose then the arm moves in X coordinate and DesirePose gets set again. angle_rad is CurrentPose’s consistent joints angles. IK’s solution for desired pose joint angles and &angle_rad get set. &ErrorStatus is the error sent to DYNAMIXEL.

ArmComm.Arm_Set_JointPosition(RobotisArm.Rad2Value(angle_rad + gvdAngleGapCalcandDynamixelRad));

The ComputeIK function sets an array for joint position in &angle_rad.

ArmKinematics.Forward(angle_rad, &CurrentPose);

Once moved to desired pose angle_rad(array) gets the end effector’s pose and runs forward kinematics; then CurrentPose sets the pose. This function returns the end effectors transform matric (4x4).

else if(temp == 'r')
{
  DesiredPose = CurrentPose;
  matd DesiredRotation = Algebra::GetOrientationMatrix(delta_angle_rad, 0.0, 0.0) *
      Algebra::GetOrientationMatrix(CurrentPose.Roll, CurrentPose.Pitch, CurrentPose.Yaw);
  vecd DesiredRPY = Algebra::GetEulerRollPitchYaw(DesiredRotation);
  DesiredPose.Roll = DesiredRPY(0);
  DesiredPose.Pitch = DesiredRPY(1);
  DesiredPose.Yaw = DesiredRPY(2);
  ArmKinematics.ComputeIK(DesiredPose, &angle_rad, angle_rad, &ErrorStatus);
  if(ErrorStatus == ARMSDK_NO_ERROR)
  {
    cout<<"Answer"<<endl;
    cout<<angle_rad<<endl<<endl;
    ArmComm.Arm_Set_JointPosition(RobotisArm.Rad2Value(angle_rad + gvdAngleGapCalcandDynamixelRad));
  }
  else if(ErrorStatus & ARMSDK_ACCEPTABLE_ERROR)
  {
    cout<< "No IK solution" <<endl;
    cout<< "But the caluation result is acceptable" <<endl;
    char answer;
    while(true)
    {
      cout<< "Do you want make the Robot move? (Y/N)";
      cin >> answer;
      if((answer == 'y') || (answer == 'n') || (answer == 'Y') || (answer == 'N'))
        break;
      else
        cout<< "Invaild Answer" <<endl;
    }
    if((answer == 'y') || (answer == 'Y') )
      ArmComm.Arm_Set_JointPosition(RobotisArm.Rad2Value(angle_rad + gvdAngleGapCalcandDynamixelRad));
    else
      continue;
  }
  else
  {
    cout<< "No IK Solution"<<endl;
    continue;
  }
  ArmKinematics.Forward(angle_rad, &CurrentPose);
}

The goal pose runs IK my moving the roll gets increased by delta(rad). The end effector moves to whatever the IK has solved and displays the joint poses(rad). Despite having errors and not being able to get the IK moving can be allowed. If ‘Do you want make the Robot move? (Y/N)’ appears onscreen press the y key to turn the end effector in the roll axis by delta_angle_rad. Then the joints pose(rad) are displayed.

A roll (roll-only) delta is ( delta_angle_rad = (3π/180)rad)
When error is too large and IK is unrealizable ‘No IK Solution‘ will be displayed the end effector will remain as is.
The sample code from above is broken down below. Press the r key to move the roll by delta_angle_rad.
The desired rotation matrix can then be obtain with the following

Where the code is shown below.

matd DesiredRotation = Algebra::GetOrientationMatrix(delta_angle_rad, 0.0, 0.0)
  *Algebra::GetOrientationMatrix(CurrentPose.Roll, CurrentPose.Pitch, CurrentPose.Yaw);

The CurrentPose’s Orientation roll increase by delta_angle_rad GoalPose(DesiredRotation).

vecd DesiredRPY = Algebra::GetEulerRollPitchYaw(DesiredRotation);

DesiredRotation’s roll, pitch, and yaw.

Simple Torque FK

Turns the Manipulator joints’ torque on/off. When torque goes off→on Forward Kinematics runs and putputs all joints pose(rad) and end effector’s position and orientation.

How to Use SimpleTorqueOnOffandFK

To start SimpleTorqueOnOffandFK start a new project just like SimplePtoP. Then press the Ctrl+F5 keys to begin.SimpleTorqueOnOffandFK.

Input the COM port and baud rate numbers. If succeeded you will see a ‘Succeed to open USB2DYNAMIXEL;’ then torque gets turned off. Press the Enter key turn torque on and the arm’s joints pose(rad) and end effector’s pose(rad) will be displayed (joints 1 through 6).

Press Enter again to turn torque off and it will display ‘Torque Off.’

Press the Enter key once again to turn torque on and the values be displayed again.

SimpleTorqueOnOffandFK Source Description

while(true)
{
  char temp = _getch();
  if(temp == 27)
  break;
  else if(temp == 13)
  {
    if(gbArmTorque)
    {
      ArmComm.Arm_Torque_Off();
      std::cout<<"Torque Off"<<std::endl;
      gbArmTorque = false;
    }
    else
    {
      ArmComm.Arm_Torque_On();
      cout<<"Torque On"<<endl;
      if(ArmComm.Arm_Get_JointPosition(&angle_unit) != COMM_RXSUCCESS)
      {
        printf("Communication Error Occurred\n");
      }
      cout<<"JointAngle is"<<endl;
      angle_rad = RobotisArm.Value2Rad(angle_unit);
      cout<< angle_rad - gvdAngleGapCalcandDynamixelRad <<endl<<endl;
      cout<<"Angle of Dynamixel is"<<endl;
      angle_rad = RobotisArm.Value2Rad(angle_unit);
      cout<< angle_rad <<endl<<endl;
      cout<<"EndEffector's Pose is"<<endl;
      Pose3D CurrentPose;
      ArmKinematics.Forward(angle_rad - gvdAngleGapCalcandDynamixelRad, &CurrentPose);
      cout<<"x = "<<CurrentPose.x <<endl;
      cout<<"y = "<<CurrentPose.y <<endl;
      cout<<"z = "<<CurrentPose.z <<endl;
      cout<<"roll = "<<CurrentPose.Roll <<endl;
      cout<<"pitch = "<<CurrentPose.Pitch <<endl;
      cout<<"yaw = "<<CurrentPose.Yaw <<endl<<endl;
      gbArmTorque = true;
    }
  }
  else
    continue;
}

The program aborts without starting by pressing the Esc key.

While the program is running press the Enter key to toggle torque between on and off. When torque gets turned on the joints and end effector pose get outputted onscreen. This happens with every “on” state.

Press the Esc key then Enter key and the arm remains as is.

The sample code from above is broken down below. The joint angles and DYNAMIXEL angles may not be the same so it must be taken into consideration. Angle of DYNAMIXEL is the output of the actual angle of DYNAMIXEL.