Edit on GitHub

Manipulator-H

Introduction

Last Updated : 5 November, 2014

Safety Information

DANGER

Information appearing in a DANGER concerns the protection of personnel from the immediate and imminent hazards that, if not avoided, will result in immediate, serious personal injury or loss of life in addition to equipment damage.

  • Keep away from the robot while its moving..
  • Do not touch with the robot with wet hands.
  • Turn off power of the robot whenever robot is problematic.

WARNING

Information appearing in a WARNING concerns the protection of personnel and equipment from potential hazards that can result in personal injury or loss of life in addition to equipment damage.

  • Setup robot in an environment low on dust and humidity.
  • The robot must always be attached to the based when powered on.
  • The robot wiring must be checked prior to powering on.
  • The robot connection to power supplly must be check prior to powering on.
  • Do not change wiring on Robotis Manipulator while powered on.

CAUTION

Information appearing in a CAUTION concerns the protection of personnel and equipment, software, and data from hazards that can result in minor personal injury or equipment damage.

  • Keep robot’s workspace clear of object.
  • Ensure wiring is not tangled up on every joint.
  • Make sure USB2Dynamixel and PC does not interfere with the robot’s moving

Package Contents

Name Quantity
Manipulator 1
USB2Dyanmixel 1
4P Cable(500mm) 2
Power Cable(1,200mm) 2
4P expansion hub 1
Power expansion hub 1
Gripper(optional) 1
Support(optional) 2
Base Plate(optional) 1
3x8 wrench bolt 20
3x12 wrench bolt 20

Layout

Dimension of Manipulator-H

Dimension of Manipulator-L

Wiring

Specifications

Item Description
DOF 6 DOF
Payload 3kg
Operating voltage 24V
Resolution Joint 1 : -π(rad) ~ π(rad) , -251000 ~ 251000 (pulse)
Joint 2 : -π(rad) ~ π(rad) , -251000 ~ 251000 (pulse)
Joint 3 : -π(rad) ~ π(rad) , -251000 ~ 251000 (pulse)
Joint 4 : -π(rad) ~ π(rad) , -251000 ~ 251000 (pulse)
Joint 5 : -π(rad) ~ π(rad) , -151875 ~ 151875 (pulse)
Joint 6 : -π(rad) ~ π(rad) , -151875 ~ 151875 (pulse)
Dynamixel Pro
Model Name
Joint 1, 2 : H54-200-S500-R
Joint 3, 4 : H54-100-S500-R
Joint 5, 6 : H42-20-S300-R
Operating Range Joint 1 : -π(rad) ~ π(rad)
Joint 2 : -π/2(rad) ~ π/2(rad)
Joint 3 : -π/2(rad) ~ 3π/4(rad)
Joint 4 : -π(rad) ~ π(rad)
Joint 5 : -π/2(rad) ~ π/2(rad)
Joint 6 : -π(rad) ~ π(rad)
Default ID Joint 1 (ID:1), Joint 2 (ID:2), Joint 3 (ID:3),
Joint 4 (ID:4), Joint 5 (ID:5), Joint 6 (ID:6)
Motor type Brushless DC Servo(H54 Series),
Coreless DC Motor(H42 Series)
Position sensor type Absolute Encoder(for Homing),
Incremental Encoder(for Control)
Communications RS485

D-H Configuration

Link Link Length(mm) Link Twist(rad) Joint Offset(mm) Joint Angle(rad) DXL Angle(rad)
1 0 -π/2 0 0 0
2 265.69 0 0 0
3 30 -π/2 0 0
4 0 -π/2 258 0 0
5 0 -π/2 0 0 0
6 0 0 0 0 0

Home Position

The diagram below shows the “home position” of the Dynamixel PROs from Robotis Manipulator.

Getting Started

Prerequisite

Preparation

Power Supply

The Manipulator requires 24V for operations. Ensure the power supply is capable of supplying 24V and 15A or higher.

4P Cable

The 4P Cable connects the Manipulator and USB2Dynamixel.

Power Cable

The power cable supplies power to the Manipulator.

For additional power or 4P cables contact ROBOTIS or obtain them with the specifications listed above. 

USB2Dynamixel

The USB2Dynamixel sends ArmSDK commands to the Manipulator. Connect the USB2Dynamixel to the PC via USB hub.

Product Assembly

Note The content below is based on an optional base plate and differs from the actual base plate.

Caution Connect the USB2Dynamixel to the PC via USB hub. The USB hub acts as an isolator to protect the PC from any possible unexpected surges caused by arm action.

USB2Dynamixel Setting

Communication Description
TTL AX, 3-pin MX; communicate with 3-pin Dynamixel
RS485 RX, 4-pin MX and Pro; communicate with 4-pin Dynamixel
RS232 CM-5, CM-510; communicate with these controllers. Communicate with other RS-232 devices

Note The manipulator is based on RS-485 communications so make sure to set the dongle to 485.

Manipulator Test

Caution Do NOT download RoboPlus v2.0. Use RoboPlus v1.0 for Manipulator-H.

Operating the Manipulator

Danger Before starting Dynamixel Wizard ensure the arm is fixed to the base plate; then extend the arm. Otherwise; it may cause physical harm.

Caution Always ensure before powering on. While power is on do not change wires; otherwise it may cause undesired operations.

Goal Position Values with Respect to Rotation

Model Name Relationship between angle(deg) and position value
H54-200-S500-R
H54-100-S500-R
-180 ~ 180 (deg) → -251000 ~ 251000

H42-20-S300-R -180 ~ 180 (deg) → -151875 ~ 151875

L54-50-S500-R -180 ~ 180 (deg) → -125700 ~ 125700

L54-30-S500-R -180 ~ 180 (deg) → -144180 ~ 144180

L42-20-S300-R -180 ~ 180 (deg) → -2048 ~ 2048

Manipulator SDK

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.

SimplePtoP

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.

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

How to Use SimplePtoP

To start SimplePtoP 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.

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

SimplePtoP 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.

SimpleIK

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 SimpleIK

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

In SimpleIK 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.

SimpleIK 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.

SimpleIK 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.

SimpleTorqueFK

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.

Manipulator SDK Programming

SDK Description

RobotInfo

When building Manipulator at ARM SDK, you may use the Addjoint function after generating Instance of RobotInfo Class.
AddJoint gets values from D-H Parameter and actuator’s max and min turn angle in rad and value as well as actuator ID number (min and max turn angles may not be the same as joint angle limits).

Kinematics

Forward Kinematics(FK), Inverse Kinematics(IK) can be calculated once the instance for kinematics class is generated. Kinematics class get the instance from RobotInfo class.
ComputeIK’s factor’s the pose from end effector and joint values, as well as, initial joint angle and error for IK. The result is joint angles when the returned error status is not 0 then the IK is not properly solved.

The Roll(φ), Pitch(θ) and Yaw(ψ) are calculated as Rx(φ), Ry(θ), Rz(ψ) in the rotation transformation matrix. This is to be taken into consideration when entering the pose for ComputeIK.

Trajectory Generating

The TrajectoryGenerator class generates an instance for the arm’s trajectory. The SDK’s Point to Point, Linear, and Circular can generate a trajectory. For arm-only trajectory then only Set_PTP, Set_LIN, Set_Circular; for the gripper then Set_PTPwithHand, Set_LINwithHand, Set_CIRCwithHand.

Velocity Profile

The SDK’s Velocity Profile does not take max velocity and max acceleration into consideration in the Trapezoidal Velocity Profile. The initial and final velocity are always set to 0. The following methods generate velocity profile in Joint Space and Cartesian Space, where both are independent of each other.

Set_PTP

The Set_PTP function determines 2 poses for the manipulator (initial and final) by factoring in Trapezoidal Velocity Profile and receives velocity time and total time. Initial and final pose are in rad and joint angle in mm or rad (x, y, z, roll, pitch, yaw). When generating the trajectory it is recommended to factor in joint angles.

Set_LIN

The Set_LIN function generates a 3-point coordinates for the robot’s straight trajectory. This factors in initial and final pose for Linear Euler Interpolation for orientation.

Set_CIRC

The Set_CIRC function generates a 3-point coordinates for the robot’s circular trajectory. This factors in initial and final pose. It sets a point of origin in the area and proceeds to trajectory via MotionPlay and vector generation.

Trajectory Following

When moving by the generated trajectory from TrajectorGenerator class’s instance just use NextStep function from MotionPlay. MotionPlay class accounts trajectoryGenerator class.
The control period from MotionPlay default value is 8ms but can be changed with SetTimePeriod. If TimePeriod is 0 then 8ms default value is applied.

Pro_Arm_Comm_Win

Pro_Arm_Comm_Win utilizes DYNAMIXEL 2.0 Protocol from the Windows version of DYNAMIXEL SDK. Pro_Arm_Comm_Win’s functions utilizes DYNAMIXEL Pro’s control (i.e. read/write Control Table values).
This is useful when writing separate code.

SDK Flowchart

Firmware Recovery

When Dynamixel detection fails ensure is properly wired. If problems persists restore Dynamixel firmware (shown below).

Warning After firmware restoration you will need to set ID and baud rate values again. Always make sure to set USB2Dynamixel switch to “485.”

  1. Restoring firmware
    • From Dynamixel Wizard click on the icon to begin.
    • Select the corresponding COM port number for USB2Dynamixel.

  2. Firmware restore process steps explained.

  3. Always connect one Dynamixel at a time.

  4. Pick the COM port number
    • With an incorrect number Dynamixel cannot be automatically detected. Always make sure to get the port number right.
    • Click on Search.

  5. Disconnect and connect Dynamixel
    • The Next button should become clickable

  6. Upon successful detection the Next button is clickable

  7. Pick the right model
    • Pick the right type from the list. If not it may result in problems

  8. During restoration
    • While restoring, the LED will blink. Do not cut power off during this stage.

All Control Table settings are set to default values.

References

AMSDK Define

Pose3D

timeprofile

MotionPose

ARMSDK Math

static matd GetOrientationMatrix(double Roll, double Pitch, double Yaw)

static matd GetTransformMatrix(double Roll, double Pitch, double Yaw, double x, double y, double z)

static vecd rot2omega(mat3d Rerr)

static vecd ConvertRad2Deg(vecd q)

static vecd GetEulerRollPitchYaw(matd T)

MotionEngine

Error.h

void ErrorCheck(int Error)

JointData.h

void SetJointID(unsigned int ID)

void SetJointAngle(double JointAngle);

void SetMinAngleInRad(double MinAngleInRad);

void SetMaxAngleInRad(double MaxAngleInRad);

void SetMinAngleInValue(int Min_AngleValue);

void SetMaxAngleInValue(int Max_AngleValue);

void SetMinAngleLimitInRad(double MinAngleLimitInRad);

void SetMaxAngleLimitInRad(double MaxAngleLimitInRad);

unsigned int GetID(void);

void SetJointDataDH(double LinkLength, double LinkTwist, double JointOffset, double JointAngle);

double GetJointAngle(void);

double GetMinAngleInRad(void);

double GetMaxAngleInRad(void);

int GetMinAngleInValue(void);

int GetMaxAngleInValue(void);

double GetMinAngleLimitInRad(void);

double GetMaxAngleLimitInRad(void);

int GetMinAngleLimitInValue(void);

int GetMaxAngleLimitInValue(void);

matd GetTransformMatirx(void);

RobotInfo.h

int AddJoint (double LinkLength, double LinkTwist, double JointOffset, double JointAngle, double MaxAngleInRad, double MinAngleInRad, int MaxAngleValue , int MinAngleValue, double MaxAngleLimitInRad, double MinAngleLimitInRad, unsigned int Dynamixel_ID);

JointData GetJointInfo(int joint_number);

std::vector* GetRobotInfo(void);

void ClearRobotInfo(void);

veci GetArmIDList(void);

veci Rad2Value(vecd q);

vecd Value2Rad(veci q);

Kinematics.h

void RobotInfoReload(void);

matd Forward(vecd angle);

matd Forward(vecd angle, Pose3D *pose);

void SetMaximumNumberOfIterationsForIK(unsigned int max_num);

void SetConvergenceCondition(double max_error, double max_acceptable_error);

matd Jacobian(void);

vecd CalcError(Pose3D _desired, matd _current);

void ComputeIK(Pose3D _desired , vecd *q_rad, vecd Initangle_rad, int *ErrorStatus);

TrajectoryGenerator.h

void KinematicsInfoReload(void);

void Set_P2P(vecd StartPose, vecd EndPose, double TotalTime, double AccelTime);

void Set_P2P(Pose3D StartPose, Pose3D EndPose, double TotalTime, double AccelTime);

void Set_LIN(vecd StartPose, vecd EndPose, double TotalTime, double AccelTime);

void Set_LIN(Pose3D StartPose, Pose3D EndPose, double TotalTime, double AccelTime);

void Set_CIRC(vecd StartPose, vecd ViaPose, vecd EndPose, double TotalTime, double AccelTime);

void Set_CIRC(Pose3D StartPose, Pose3D ViaPose, Pose3D EndPose, double TotalTime, double AccelTime);

void Set_P2PwithHand(vecd StartPose, vecd EndPose, veci Hand1, veci Hand2, double TotalTime, double AccelTime);

void Set_P2PwithHand(Pose3D StartPose, Pose3D EndPose, veci Hand1, veci Hand2, double TotalTime, double AccelTime);

void Set_LINwithHand(vecd StartPose, vecd EndPose, veci Hand1, veci Hand2, double TotalTime, double AccelTime);

void Set_LINwithHand(Pose3D StartPose, Pose3D EndPose, veci Hand1, veci Hand2, double TotalTime, double AccelTime);

void Set_CIRCwithHand(vecd StartPose, vecd ViaPose, vecd EndPose, veci Hand1, veci Hand2, double TotalTime, double AccelTime);

void Set_CIRCwithHand(Pose3D StartPose, Pose3D ViaPose, Pose3D EndPose, veci Hand1, veci Hand2, double TotalTime, double AccelTime);

void ClearMF(void)

double GetMotionTotalTime(void)

MotionPlay.h

void All_Info_Reload(void);

void initialize(void)

void Set_Time_Period(int MilliSecond)

vecd NextStepAtTime(double CurrentTime, int *ErrorStatus)

veci NextStepAtTimeforHand(double CurrentTime)

vecd NextStep(int* ErrorStatus)

veci NextStepforHand(void)

vecd GetCurrentAngle(void);

Pose3D GetCurrentEndPose(void);

double Get_CurrentTime(void);

RobotisLib

Pro_Arm_Comm_win.h

void DXL_Set_Init_Param(int portnum, int baudnum);

int DXL_Open();

SerialPort* DXL_Get_Port(void);

void DXL_Close(void);

void Arm_ID_Setup(veci Arm_ID_LIST);

int Arm_Torque_On(void);

int Arm_Torque_Off(void);

int Arm_Set_JointPosition(veci position);

int Arm_Set_JointVelocity(veci velocity);

int Arm_Set_JointVelocity(int velocity);

int Arm_Set_JointAcceleration(veci accel);

int Arm_Set_JointAcceleration(int accel);

int Arm_Set_Position_PID_Gain(int P_Gain, int I_Gain, int D_Gain);

int Arm_Set_Position_PID_Gain(int id, int P_Gain, int I_Gain, int D_Gain, int* ErrorStatus);

int Arm_Get_JointPosition(veci *position);

int Arm_Get_JointCurrent(veci *torque);

int Arm_LED_On(void);

int Arm_LED_Off(void);

int Arm_LED_On(int r, int g, int b);

int Arm_Red_LED_On(void);

int Arm_Green_LED_On(void);

int Arm_Blue_LED_On(void);

void Gripper_ID_Setup(veci Gripper_ID_List);

int Gripper_Ping(void);

int Gripper_Torque_On(void);

int Gripper_Torque_Off(void);

int Gripper_Get_Joint_Value(veci *value);

int Gripper_Set_Joint_Value(veci value);

Timer

MotionTimer.h

time measurement fromQueryPerformanceCounter

void Start(void)

void Stop(void)

double GetElapsedTime(void)

void Wait(double millisec)

Mass Property

Coordinate

Total Mass : 5,551g