void RosSchunk::closeHand(double percentage, double velocity) {
if(percentage >= 0.0 && percentage <= 1.1) {
vector<double> hand_pose;
switch(currentGraspId) {
case eGID_CENTRICAL:
hand_pose = generateCentricalPose(percentage);
break;
case eGID_CYLINDRICAL:
hand_pose = generateCylindricalPose(percentage);
break;
case eGID_PARALLEL:
hand_pose = generateParallelPose(percentage);
break;
case eGID_SPHERICAL:
hand_pose = generateSphericalPose(percentage);
break;
default:
string msg = "(RosSchunk) grasp type not supported";
cerr << msg << endl;
throw KukaduException(msg.c_str());
}
moveJoints(stdToArmadilloVec(hand_pose));
} else {
string msg = "(RosSchunk) grasp percentage out of range";
cerr << msg << endl;
throw KukaduException(msg.c_str());
}
}
void RosSchunk::publishSingleJoint(int idx, double pos) {
vector<double> command;
for(int i = 0; i < currentCommandedPos.size(); ++i)
if(i == idx)
command.push_back(pos);
else
command.push_back(SDH_IGNORE_JOINT);
moveJoints(stdToArmadilloVec(command));
}
RosSchunk::RosSchunk(ros::NodeHandle node, std::string type, std::string hand) {
this->node = node;
waitForReached = true;
trajPub = node.advertise<std_msgs::Float64MultiArray>(string("/") + type + string("/") + hand + "_sdh/joint_control/move", 1);
this->hand = hand;
stateSub = node.subscribe(string("/") + type + string("/") + hand + "_sdh/joint_control/get_state", 1, &RosSchunk::stateCallback, this);
tactileSub = node.subscribe(string("/") + type + string("/") + hand + "_sdh/sensoring/tactile", 1, &RosSchunk::tactileCallback, this);
previousCurrentPosQueueSize = 10;
isFirstCallback = true;
ros::Rate r(5);
while(isFirstCallback) {
ros::spinOnce();
r.sleep();
}
currentCommandedPos = currentPos;
currentGraspId = eGID_PARALLEL;
moveJoints(stdToArmadilloVec(currentPos));
}
INT_PTR CALLBACK gait_dia(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
{
int wmId, wmEvent;
switch (message)
{
case WM_INITDIALOG:
{
/* TODO: Fill the gait textbox with gait names */
HWND hListBox = GetDlgItem(hDlg, IDC_LIST1);
ListBox_ResetContent(hListBox);
for(int i = 0; i < g_numGaits; i++) {
ListBox_AddString(hListBox, g_gaits[i]->getName());
}
break;
}
case WM_COMMAND:
wmId = LOWORD(wParam);
wmEvent = HIWORD(wParam);
// Parse the menu selections:
switch (wmId)
{
case IDC_BUTTON1: /* Play Gait button clicked */
{
/* Get the index of the gait to play */
HWND hListBox = GetDlgItem(hDlg, IDC_LIST1);
int index = ListBox_GetCurSel(hListBox);
if(index == LB_ERR) {
return TRUE;
}
int numMotions = g_gaits[index]->getNumMotions();
const Motion *motion;
for(int i = 0; i < numMotions; i++) {
motion = g_gaits[index]->getMotion(i);
if(motion->getType() == MOTION_POSE) {
poseJoints(motion->getAngles(), motion->getMotorMask());
} else if (motion->getType() == MOTION_MOVE) {
moveJoints(motion->getAngles(), motion->getMotorMask());
}
moveWait();
}
break;
}
case IDC_BUTTON2: /* New Gait button clicked */
{
DialogBox(g_hInst, (LPCTSTR)IDD_NEWGAIT, hDlg, newgait_dia);
HWND hListBox = GetDlgItem(hDlg, IDC_LIST1);
ListBox_ResetContent(hListBox);
for(int i = 0; i < g_numGaits; i++) {
ListBox_AddString(hListBox, g_gaits[i]->getName());
}
break;
}
case IDC_BUTTON3: /* Cancel button clicked */
{
EndDialog(hDlg, LOWORD(wParam));
return TRUE;
}
}
break;
case WM_CLOSE:
EndDialog(hDlg, message);
return TRUE;
}
return (INT_PTR)FALSE;
}
