====== DASL Tutorial HDT ARM #2 “How to move fingers while moving to position” in C/C++ ====== //This tutorial discusses using the HDT 3-fingered gripper. The first objective is to grasp objects with manual loading and placement in specific locations by user input, demonstrated here: // [[https://youtu.be/1e8e9wP46ao?si=e23jONY5Jvnu8-W8|Manual loading and user input placement]] //, and the second is for pick and place objects, shown here: // [[https://www.youtube.com/watch?v=u-lvnkA1rGI|User input grasping and placement]]. {{ :hdt_gripper2.png?direct&400 |}} ==== Authors ==== //[[kjmean0112@naver.com|Min Kim]]//, //[[dowdj1@unlv.nevada.edu|Jacob Dowd]]// ==== Pre-requisites ===== * DASL HDT arm tutorial #1 [[https://www.daslhub.org/unlv/wiki/doku.php?id=dasl_tutorial_hdt_arm_1_move_home_desired_position|How to move from Home to desired (x,y,z) position” in C/C++]] (necessary) * Other pre-requisites listed in the above tutorial ==== Tutorial completion time ==== 1-2 hrs ==== Expectations ==== * You **will** have to change some coding lines in this tutorial, it is recommended that you use "ctrl+f" searching //tutorial2// in the following document: {{ :mk2_ik_getting_offset.cpp_final_.pdf |}}, to determine the location of the coding adjustments. * Try not to remove any lines in the coding document **"mk2_ik_getting_offset.cpp"**, but if you do, know that the lines referenced in this tutorial may be slightly off. If so, just compare the listed lines to the document in the previous bullet point. ==== Note for safe working conditions ==== 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. ---- ===== How to control the HDT arm and gripper ===== //This section covers the changes that need to be made when switching from the previous HDT tutorial #1 to this tutorial. It is common to two following sections that cover the slightly different tasks involved.// We need to make some edits to **"mk2_ik_getting_offset.cpp"** before we launch the software. Referencing, **"mk2_ik_getting_offset.cpp"**, you may search in the document "tutorial2" to find the four changes, which are listed below: ==== Change #1 ==== The coding in this program is similar to Tutorial#1’s coding but before we made the robot just move on the position we entered. So, we expand the group numbers. To define the group members we use the line: 72 const std::vector&joint_names1=kinematic_state->getJointModelGroup(“manipulator 1”)->getJointModelNames(); === Breakdown of the above line === 'const std::vector& joint_names1 = ' // reference to a vector holding joint names that are strings 'kinematic_state->getJointModelGroup("manipulator 1")->getJointModelNames();' // object 'kinematic_state' points to model group which points to the names of the joint model vector of strings. To use ‘const std:: vector’ you can handle the components as vectors. ==== Change #2 ==== 81 joint_names_global.push_back(joint_names1.at(i)); ==== Change #3 ==== To define the constant velocity to the robot components you have to define the new array 'p1[]', but don't delete the previously defined 'p[]' array. 95 double p1[] = {0.18,0.18,0.15,0.18,0.18,0.18,0.18,0.15,0.18,0.18, 0.1, 0.1, 0.1, 0.1}; 96 std::vector vel1(p1, p1+14); **Note that p1[2]=0.05, p1[7]=0.05 are 0.03 less than others, maintain this difference for increasing or decreasing arm speed.** For changing gripper speed, change p1[10], p1[11], p1[12], and p1[13], directly. //We should expand the group because previously the robot did not have a hand or fingers.// __Reference : 9th vector = wrist , 10th vector: hand rotation, 11~14 vectors = fingers __ You can change the speed of arm or grabbing velocity. Just change the number above the vectors. //Ideally, fast arm movement with slow gripping movement.// ==== Change #4 ==== Remove opening and closing comments on 178 & 220, respectively. [This block defines the HDT hand] 178 kinematic_state->copyJointGroupPositions(joint_state_group, joint_values); 179 joint_values.push_back(0.2); // index 180 joint_values.push_back(0.2); //ring yaw 181 joint_values.push_back(-0.3); //angle thumb 182 joint_values.push_back(0.2); //thumb 183 184 msg.header.stamp = ros::Time::now(); 185 msg.name = joint_names1; 186 187 msg.position = joint_values; 188 msg.velocity = vel1; 189 msg.header.stamp = ros::Time::now(); 190 msg.name = joint_names1; 191 msg.name.push_back("right_thumb_roll_joint"); 192 msg.name.push_back("right_thumb_pitch_joint"); 193 msg.name.push_back("right_index_yaw_joint"); 194 msg.name.push_back("right_ring_yaw_joint"); 195 joint_pub_.publish(msg); 196 } 197 else 198 { 199 homePose_flag = false; 200 201 kinematic_state->copyJointGroupPositions(joint_state_group, joint_values); 202 203 joint_values.push_back(1.5); // index 204 joint_values.push_back(1.5); //ring yaw 205 joint_values.push_back(-1.5); //angle thumb 206 joint_values.push_back(1.2); //thumb 207 208 msg.header.stamp = ros::Time::now(); 209 msg.name = joint_names1; 210 211 msg.position = joint_values; 212 msg.velocity = vel1; 213 msg.header.stamp = ros::Time::now(); 214 msg.name = joint_names1; 215 msg.name.push_back("right_thumb_roll_joint"); 216 msg.name.push_back("right_thumb_pitch_joint"); 217 msg.name.push_back("right_index_yaw_joint"); 218 msg.name.push_back("right_ring_yaw_joint"); 219 joint_pub_.publish(msg); 220 } ==== Change #5 ==== 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 ==== Concept ==== In contrast to the previous tutorial, we want the hand xy-plane parallel with the workspace plane, referencing the picture below: {{ :end_effector_coordinates.png?nolink&400 |}} Now we can modify the robot arm moving on to the position which entered, same as in the previous tutorial, but now, one can use the robot fingers. This launch file is named min_proj.launch. The launch file leads the robot to cpp command. Let’s check the cpp file. The cpp file name is "mk2_ik_getting_offset.cpp" ---- ===== Manual loading and user input placement ===== * //This section covers the changes that need to be made so that the gripper can be manually loaded and released by user input.// ‘joint_values.push_back();’ Also, you should make coding to designate vectors. The vectors belong in a group named ‘joint_value1’ we made already. (If you want to change, you can change the name.) Then, do not forget joint_pub_.publish(msg); which shows the robot's current state. Let's see the above coding line of 'if'. I intend that by adding a small value in the ‘x=0’ line, the robot will grasp. The structure in the ‘else’ line is the case where the robot is being controlled by the user input. **The reason for the different values, when departing from home, the robot is going to grab slowly. Also, entering (0,0,0) can release the object while going to the home position.** {{ :hdt_arm:kakaotalk_20231108_130709851_02.jpg?400 |}} In conclusion, you can increase the robot speed or decrease grabbing speed. So, if you combine with tutorial#1, you can pick up some stuff. We need the robot to grab slowly and with fast joint movements because the action is simultaneous. Therefore, you can give another function in accordance with your intention. For example, you can give the robot some sleep before the robot moves which way can grab after the robot gets position. As before, to enact changes to the program, do not forget to ‘make.’ Now I just posted a video with content on changing velocity of arm and grabbing. Increasing velocity of arm and decreasing grabbing speed makes grab the stuff on the table. https://youtu.be/Ngw0I5fa4ek?si=Sei6JKsGU61TH2Lh ---- ===== User input grasping and placement ===== * // This section is a direct upgrade to the previous section, but for grasping the object, it needs to be placed along the line x = 0.5, that is, the line parallel to the horizontal (long component of the workspace).// ==== Change #6 ==== The following program lines show that when the robot arm is at position x = 0.5, the fingers are **not** a grasping position. 222 if(ad_x == 0.5) 223 { 224 homePose_flag = false; 225 //ROS_INFO("If you entered z < 0.28, plz enter z > 0.28"); 226 kinematic_state->copyJointGroupPositions(joint_state_group,joint_values); 227 joint_values.push_back(0.2); // index 228 joint_values.push_back(0.2); //ring yaw 229 joint_values.push_back(-0.3);//angle thumb 230 joint_values.push_back(0.2); //thumb 231 232 msg.header.stamp = ros::Time::now(); 233 msg.name = joint_names1; 234 235 msg.position = joint_values; 236 msg.velocity = vel1; 237 msg.header.stamp = ros::Time::now(); 238 msg.name = joint_names1; 239 msg.name.push_back("right_thumb_roll_joint"); 240 msg.name.push_back("right_thumb_pitch_joint"); 241 msg.name.push_back("right_index_yaw_joint"); 242 msg.name.push_back("right_ring_yaw_joint"); 243 joint_pub_.publish(msg); 244 } Now, if you modify the robot to (x,y,z)=(0.5,0.5,0.2), you can move on another y position with a nearby table. If you want to grab an object at the position: (0.7,0.7,0.1), you can move on from (0.5,0.7,0.1) position. After grasping, you can move to any position besides the line x=0.5 to release. See the video below for the description, linked in the beginning as well: [[https://youtu.be/u-lvnkA1rGI?si=9IPTdoC-EkJ8vl3V|User input grasping and placement]]