The purpose of this tutorial is to teach a beginner how to control the HDT arm to move from one position to another position that is input by the user shown here: HDT Arm Movement Tutorial #1.

Min Kim, Jacob Dowd

• C++ (some knowledge is necessary)
• ROS (some knowledge is helpful)
• Basic Linux terminal navigation
• How to Move A HDT MK2 Arm: hdt_setup

<1 hr

• The coding file is changed between this tutorial and the next, so referenced lines numbers may change slightly, to counteract this, we made the following pdf document for an exact reference, so you should download the following document mk2_ik_getting_offset.cpp_final_.pdf which directly matches the code needed for this tutorial.
• Since the coding file is changed between this tutorial and the next, you may have to undo the changes made to the coding file shown as the document referenced above and make them match.

Please do not crash the HDT arm into anything, so keep the workspace clear of any obstacles. If this cannot be avoided, or other unsafe conditions arise, press 'Ctrl+C' as an e-stop, on the keyboard.

First, you should turn on the power source, found underneath the HDT workspace.

This will take you to the proper file location:

$  cd /catkin_ws/src/mk2_controller/launch
$  roslaunch min_proj.launch // while in the above directory

Running this launch file allows you to move to a desired position. When you run this launch file named min_proj.launch, it will run Rviz to visualize the current state of the robot, as well as the main C++ script to control the arm. The launch file leads to the terminal control of the robot.

Let’s check the cpp file. The cpp file name is: mk2_ik_getting_offset.cpp

110     while(nh_.ok())
111     {
112     
113     
114     if(homePose_flag)
115     {
116          tf::poseEigenToMsg(end_effector_state, actual_end_effector_);
117          msg.header.stamp = ros::Time::now();
118          msg.name = joint_names;
119          msg.position = initial_joint_values;
120          
121          msg.velocity = vel;
122          joint_pub_.publish(msg);
123     }

Note that any two positions equal to zero sets the 'homePose_Flag' to go to the home position, e.g. (X,Y,Z) = (0, 0, 0.5) → homePose_Flag == true.

The if statement above checks if the robot is in its home position, we can send a new position.

124      //finding setfromik, changing jointstategroup
125      else
126      {
127           tf::poseMsgToEigen(actual_end_effector_, new_end_effector_state);
128          
129           bool found_ik = kinematic_state->setFromIK(joint_state_group,
130                new_end_effector_state, 10, 0.1);
131          
132           if (found_ik) //sets up the motion
133           {
134                kinematic_state->copyJointGroupPositions(joint_state_group, joint_values);
135               
136                msg.header.stamp = ros::Time::now();
137                msg.name = joint_names;
138                msg.position = joint_values;
139                msg.velocity = vel;
140                joint_pub_.publish(msg);
141               
142               
143               
144                std::cout << "\n Each joint target: \n";
145                std::cout << "joint: ";
146                std::copy(joint_values.begin(), joint_values.end(), std::ostream_iterator<double>(std::cout, "\n joint: "));
147                std::cout << "\n";
148           }
149           else
150           {
151                ROS_INFO("Did not find IK solution");
152           }
153      }

The sentence ‘tf::poseMsgToEigen’ got a new effect at the end of coding. In the case that the HDT arm is not in the home position, this 'else' statement sets the position for a found solution. Now, as seen in the image below, the robot then is in idle, waiting for an X, Y, and Z coordinate, since the while loop condition to repeatedly run is 'nh_.ok()' ('Ctrl+C' is not entered, or other failsafe conditions not met). Since we have 'std::cin' stuck in a while loop, the program waits for the user to input the coordinate positions and then repeats until the user uses 'Ctrl+C' to end the program.

After putting in the X, Y, and Z coordinate, the next iteration of the while loop will then enter the else case (no longer in home position). Continually, another ‘if’ function came. It divides whether you got ‘found IK’ from a new effect or not. If the robot continues, it is because 'found_ik' is true, if it is not true, then an IK solution is not found, and the arm cannot continue.

Typing position structure / Adjusting wrist angle.

154      std::cout << "Hi I`m Min \n"; // terminal begin output
155      float ad_x = 0.295; // x initial value
156      // Y&Z is opposite direction
157      float ad_y = -0.249; // y initial value
158      float ad_z = -0.5; // z initial value
159      // float joint_8 = -0.5;
// 160-164 spaced lines
165      homePose_flag = false;
166      std::cin >> ad_x; // user input position in x ,y(line 167),z(line 168)
167      std::cin >> ad_y; 
168      std::cin >> ad_z; 
169      
170      
171      std::cout << " final point: " << ad_x << "," << ad_y << "," << ad_z << "\n"; // print out input values

Below the sentence means that if you enter x=0, the robot goes to homepose_flag(=home). Then, the robot receives the position, it will go to the position using the line ’actual_end_effector_.position.(x,y,z).'

173      if(ad_x == 0)
174      {
175           homePose_flag = true;
176           ROS_INFO("If you entered z < 0.28, plz enter z > 0.28"); // safety z
// skip 177-251 (comments)
252           ad_x += (-0.2); // x offset
253           ad_y += (0);
254           ad_z += (0);
255           //'joint_6' += (0);
256           //change Y&Z axis direction
257           ad_y = (-ad_y);
258           ad_z = (-ad_z);
259           //'joint_6' = (-'joint_6');
260           //std::cout<< x_ad;
261           
262           
263           actual_end_effector_.position.x = ad_x; // set input values to 
264           actual_end_effector_.position.y = ad_y;
265           actual_end_effector_.position.z = ad_z;

Also, we can adjust wrist angle. This used the concept of Quaternion, see the video for some background: https://www.youtube.com/watch?v=zjMuIxRvygQ

267           actual_end_effector_.orientation.x = 0.0; // end-effector x-position
268           actual_end_effector_.orientation.y = -0.7071068; // end-effector y-position
269           actual_end_effector_.orientation.z = 0.0; // end-effector z-position
270           actual_end_effector_.orientation.w = 0.7071068; // end-effector w-orientation

For better understanding of the above program lines, 267-270, consider a cylinder that has position x,y,z, and can rotate, w. One can think of the end of the robot arm, the end-effector control, as the cylinder that has positions and rotation.

Then consider an inertial plane as shown in the picture below, that is, an airplane “pitching” by the Y-axis is going to collide with the Z-axis. Or simply, in the context of a robot arm, any rotation about a joint that is too great may lead to the robot arm colliding with another part of the arm. So, to prevent this from happening we can use quaternion.

If you want to change the angle, you can use a quaternion calculator: https://www.andre-gaschler.com/rotationconverter/. Though it is not covered in depth here, you can change the orientation of the “end-effector” with this calculator and set the previous coding 'actual_end_effector_.orientation.x', for example, according to the calculator values. Now, the coding makes operating wrist parallel with table. On the other hand, if putting 0,0,0,0 on quaternion values, it makes operating wrist vertical with table: the cylinder's zy-plane will be parallel with the workspace plane.

You can see the wrist head perpendicular/vertical to the table. If you want to give another function to use, you can change the hands tool. The current setup is good for a downward facing pick and place task using a granular jamming gripper for example. Specifically, this is the xz-plane parallel with the workspace plane.

First, choose an update file. Coming from the 'launch' directory, choose the 'mk2_controller' directory using:

To shift back to the previous directory. And then, using following command in the command line

$  make

When you clicked make, you updated the coding to robot. If the 'make' failed, perhaps there is an error in the current configuration. Ignoring any warnings, if the prompt shows a 100% build, then it is ready to use.

Note that you must 'make' the project again with each change that you have, or else you are using an old version of the program