Originally Posted by
ShirleyWang
And the value can be changed in step 7 which shows the current value of servos. But it seems not enough. So, maybe cannot add data of new servos into motion_4096.bin file?
Shirley
I've actually done this before on some of the custom "Jimmy" robots we built over the course of the 21st Century Robot project, so I can tell you it does indeed work, it's just a bit cumbersome due to how the Darwin-OP framework was originally coded- most of the servo ID handling is hardcoded for 20 servos/darwin configuration, without any thought into scalability.
I believe you also have to initialize each page as 'new' in rme to get them to respond, otherwise they have the old servo ID tables. The way the binary motion file is handled is poorly documented to say the least.
I haven't actually done this in awhile, but you'll see some remnants of my code in the HROS5-Framework. Renee has some of it already and it sounds like you've figured out some, but here are my notes on what needs to be updated:
JointData.h
Update ID enum of JointData.h to include the new servos.
Code:
ID_L_ELBOW_YAW = 21,
ID_R_ELBOW_YAW = 22,
ID_MAX = 22,
NUMBER_OF_JOINTS = 23 //This is always ID_MAX + 1
JointData.cpp
If you want to be able to properly execute motion module handoffs, you need to update the new joints into one of the enable functions in JointData.cpp So if you added servos to the arms, you'd want to update the corresponding functions:
Code:
void JointData::SetEnableRightArmOnly(bool enable, bool exclusive)
{
SetEnable(ID_R_SHOULDER_PITCH, enable, exclusive);
SetEnable(ID_R_SHOULDER_ROLL, enable, exclusive);
SetEnable(ID_R_ELBOW, enable, exclusive);
SetEnable(ID_R_ELBOW_YAW, enable, exclusive);
SetEnable(ID_R_WRIST_YAW, enable, exclusive);
SetEnable(ID_R_GRIPPER, enable, exclusive);
}
dxl_monitor
This one is pretty easy as it populates based on JointData.h so most of the work is already done. You do need to tell it what string label/name you want it to output when it detects a recognized servo in the framework though. Here's changes to cmd_process.cpp for example:
Code:
case JointData::ID_HEAD_TILT:
return "HEAD_TILT";
case ArbotixPro::ID_CM:
return "ARBOTIX_PRO";
case JointData::ID_TORSO_ROTATE:
return "TORSO_ROTATE";
case JointData::ID_R_ELBOW_YAW:
return "R_ELBOW_YAW";
case JointData::ID_L_ELBOW_YAW:
return "L_ELBOW_YAW";
case JointData::ID_R_WRIST_YAW:
return "R_WRIST_YAW";
case JointData::ID_L_WRIST_YAW:
return "L_WRIST_YAW";
case JointData::ID_R_GRIPPER:
return "R_GRIPPER";
case JointData::ID_L_GRIPPER:
return "L_GRIPPER";
rme/action_editor
This gets a little trickier as there is a LOT hardcoded.
cmd_process.cpp change example where I've added 6 servos for a total of 26, and renamed all servo IDs via JointData.h
line 345~ in the DrawPage function
Code:
int res = system("clear"); res = 0;
// 80 01234567890123456789012345678901234567890123456789012345678901234567890123456789 //24
printf( "ID: 1(RR_COXA) [ ] \n" );//0
printf( "ID: 2(RR_FEMUR) [ ] Page Number: \n" );//1
printf( "ID: 3(RR_TIBIA) [ ] Address: \n" );//2
printf( "ID: 4(RR_TARSUS) [ ] Play Count: \n" );//3
printf( "ID: 5(RM_COXA) [ ] Page Step: \n" );//4
printf( "ID: 6(RM_FEMUR) [ ] Page Speed: \n" );//5
printf( "ID: 7(RM_TIBIA) [ ] Accel Time: \n" );//6
printf( "ID: 8(RM_TARSUS) [ ] Link to Next: \n" );//7
printf( "ID: 9(RF_COXA) [ ] Link to Exit: \n" );//8
printf( "ID:10(RF_FEMUR) [ ] \n" );//9
printf( "ID:11(RF_TIBIA) [ ] \n" );//0
printf( "ID:12(RF_TARSUS) [ ] \n" );//1
printf( "ID:13(LR_COXA) [ ] \n" );//2
printf( "ID:14(LR_FEMUR) [ ] \n" );//3
printf( "ID:15(LR_TIBIA) [ ] \n" );//4
printf( "ID:16(LR_TARSUS) [ ] \n" );//5
printf( "ID:17(LM_COXA) [ ] \n" );//6
printf( "ID:18(LM_FEMUR) [ ] \n" );//7
printf( "ID:19(LM_TIBIA) [ ] \n" );//8
printf( "ID:20(LM_TARSUS) [ ] \n" );//9
printf( "ID:21(LF_COXA) [ ] \n" );//0
printf( "ID:22(LF_FEMUR) [ ] \n" );//1
printf( "ID:23(LF_TIBIA) [ ] \n" );//2
printf( "ID:24(LF_TARSUS) [ ] \n" );//3
printf( "ID:25(HEAD_PAN) [ ] \n" );//4
printf( "ID:26(HEAD_TILT) [ ] \n" );//5
printf( " PauseTime [ ] \n" );//6
Next you need to update cmd_process.h so it draws more rows in the terminal to account for additional servo, example:
line 8~
Code:
#define SCREEN_COL 80
#define SCREEN_ROW 30 //original value was 24, + 6 for additional servos.
line 31~
Code:
// Position of Row
#define ID_1_ROW 0
#define ID_2_ROW 1
#define ID_3_ROW 2
#define ID_4_ROW 3
#define ID_5_ROW 4
#define ID_6_ROW 5
#define ID_7_ROW 6
#define ID_8_ROW 7
#define ID_9_ROW 8
#define ID_10_ROW 9
#define ID_11_ROW 10
#define ID_12_ROW 11
#define ID_13_ROW 12
#define ID_14_ROW 13
#define ID_15_ROW 14
#define ID_16_ROW 15
#define ID_17_ROW 16
#define ID_18_ROW 17
#define ID_19_ROW 18
#define ID_20_ROW 19
//new servos here
#define ID_21_ROW 20
#define ID_22_ROW 21
#define ID_23_ROW 22
#define ID_24_ROW 23
#define ID_25_ROW 24
#define ID_26_ROW 25
//pause & speed row are sequentially enumerated from the prior value
//(0 through 25 in this case, 0 through 19 on stock darwin)
#define PAUSE_ROW 26 //originally 20
#define SPEED_ROW 27 //originally 21
#define CMD_ROW 29 //originally 23 (this is always SPEED_ROW + 2)
I believe that's it. Any other terminal programs would need to be updated in a similar manner. Let me know if this works for you.
HR-OS1 Has anyone added additional servos ？
Reply With Quote
Bookmarks