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; }