From ac10465ab6b44451d7fbae518acd94035d6c40ef Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Mon, 2 Sep 2019 20:13:16 -0400 Subject: [PATCH 1/6] Re-added the launch file accidentally deleted from the last PR --- ambf_models/descriptions/launch.yaml | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 ambf_models/descriptions/launch.yaml diff --git a/ambf_models/descriptions/launch.yaml b/ambf_models/descriptions/launch.yaml new file mode 100644 index 000000000..3d9ced643 --- /dev/null +++ b/ambf_models/descriptions/launch.yaml @@ -0,0 +1,25 @@ +# This is the base file for Coordination Application +world config: ./world/world.yaml +# world config: ./world/raven_world.yaml +color config: ./color/colors.yaml +input devices config: ./input_devices/input_devices.yaml +multibody configs: + - "./multi-bodies/robots/blender-toy-car.yaml" #0 + - "./multi-bodies/robots/blender-toy-car2.yaml" #1 + - "./multi-bodies/robots/blender-neuro-robot.yaml" #2 + - "./multi-bodies/robots/blender-raven2.yaml" #3 + - "./multi-bodies/robots/blender-mtm.yaml" #4 + - "./multi-bodies/robots/blender-psm.yaml" #5 + - "./multi-bodies/robots/blender-ecm.yaml" #6 + - "./multi-bodies/robots/blender-suj.yaml" #7 + - "./multi-bodies/robots/blender-kuka.yaml" #8 + - "./multi-bodies/robots/blender-pr2.yaml" #9 + - "./multi-bodies/puzzles/parallel_structure.yaml" #10 + - "./multi-bodies/puzzles/puzzle1.yaml" #11 + - "./multi-bodies/puzzles/puzzle2.yaml" #12 + - "./multi-bodies/robots/blender-pr2-textured.yaml" #13 + - "./multi-bodies/robots/sophie.yaml" #14 + - "./multi-bodies/puzzles/puzzle4.yaml" #15 + - "./multi-bodies/puzzles/puzzle5.yaml" #16 + - "./multi-bodies/puzzles/puzzle6.yaml" #17 + From 40fa76078fdb7ff678add4dbe0cebec4fc0a2e81 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Mon, 2 Sep 2019 20:17:04 -0400 Subject: [PATCH 2/6] Using mutexes for thread safe update of Position and Rotation References --- ambf_framework/afInputDevices.cpp | 43 ++++- ambf_framework/afInputDevices.h | 20 ++- ambf_simulator/src/ambf_simulator.cpp | 232 +++++++++++++------------- 3 files changed, 173 insertions(+), 122 deletions(-) diff --git a/ambf_framework/afInputDevices.cpp b/ambf_framework/afInputDevices.cpp index 5522260d0..e938bac70 100644 --- a/ambf_framework/afInputDevices.cpp +++ b/ambf_framework/afInputDevices.cpp @@ -68,6 +68,45 @@ afSharedDataStructure::afSharedDataStructure(){ m_rotRefOrigin.identity(); } + +/// +/// \brief afSharedDataStructure::setPosRef +/// \param a_pos +/// +void afSharedDataStructure::setPosRef(cVector3d a_pos){ + std::lock_guard lock(m_mutex); + m_posRef = a_pos; +} + + +/// +/// \brief afSharedDataStructure::setRotRef +/// \param a_rot +/// +void afSharedDataStructure::setRotRef(cMatrix3d a_rot){ + std::lock_guard lock(m_mutex); + m_rotRef = a_rot; +} + + +/// +/// \brief afSharedDataStructure::getPosRef +/// \return +/// +cVector3d afSharedDataStructure::getPosRef(){ + std::lock_guard lock(m_mutex); + return m_posRef; +} + +/// +/// \brief afSharedDataStructure::getRotRef +/// \return +/// +cMatrix3d afSharedDataStructure::getRotRef(){ + std::lock_guard lock(m_mutex); + return m_rotRef; +} + /// /// \brief afPhysicalDevice::~afPhysicalDevice /// @@ -372,9 +411,9 @@ bool afPhysicalDevice::loadPhysicalDevice(YAML::Node *pd_node, std::string node_ m_simRotInitial.identity(); } - simDevice->m_posRef = _position/ m_workspaceScale; + simDevice->setPosRef(_position/ m_workspaceScale); simDevice->m_posRefOrigin = _position / m_workspaceScale; - simDevice->m_rotRef = _orientation; + simDevice->setRotRef(_orientation); simDevice->m_rotRefOrigin = _orientation; m_gripper_pinch_btn = 0; diff --git a/ambf_framework/afInputDevices.h b/ambf_framework/afInputDevices.h index 9caf5d79f..fb034a25f 100644 --- a/ambf_framework/afInputDevices.h +++ b/ambf_framework/afInputDevices.h @@ -86,10 +86,22 @@ struct afSharedDataStructure{ afSharedDataStructure(); public: - cVector3d m_posRef; cVector3d m_posRefOrigin; - cMatrix3d m_rotRef; cMatrix3d m_rotRefOrigin; + +public: + void setPosRef(cVector3d a_pos); + void setRotRef(cMatrix3d a_rot); + + cVector3d getPosRef(); + cMatrix3d getRotRef(); + +private: + cVector3d m_posRef; + cMatrix3d m_rotRef; + +protected: + std::mutex m_mutex; }; /// @@ -128,8 +140,8 @@ class afSimulatedDevice: public afSharedDataStructure, public afMultiBody{ // Root link for this simulated device hhhhhhh afRigidBodyPtr m_rootLink; -private: - std::mutex m_mutex; +//private: +// std::mutex m_mutex; }; diff --git a/ambf_simulator/src/ambf_simulator.cpp b/ambf_simulator/src/ambf_simulator.cpp index df8d1cbf1..67fd19e54 100644 --- a/ambf_simulator/src/ambf_simulator.cpp +++ b/ambf_simulator/src/ambf_simulator.cpp @@ -1306,34 +1306,34 @@ void updatePhysics(){ } for (unsigned int devIdx = 0 ; devIdx < g_inputDevices->m_numDevices ; devIdx++){ // update position of simulate gripper - afSimulatedDevice * simGripper = g_inputDevices->m_psDevicePairs[devIdx].m_simulatedDevice; - afRigidBodyPtr rootLink = simGripper->m_rootLink; - simGripper->updateMeasuredPose(); + afSimulatedDevice * simDev = g_inputDevices->m_psDevicePairs[devIdx].m_simulatedDevice; + afRigidBodyPtr rootLink = simDev->m_rootLink; + simDev->updateMeasuredPose(); if (g_enableGrippingAssist){ for (int sIdx = 0 ; sIdx < rootLink->getAFSensors().size() ; sIdx++){ afSensorPtr sensorPtr = rootLink->getAFSensors()[sIdx]; if (sensorPtr->m_sensorType == afSensorType::proximity){ afProximitySensor* proximitySensorPtr = (afProximitySensor*) sensorPtr; - if (proximitySensorPtr->isTriggered() && simGripper->m_gripper_angle < 0.5){ + if (proximitySensorPtr->isTriggered() && simDev->m_gripper_angle < 0.5){ if (proximitySensorPtr->m_sensedBodyType == afProximitySensor::RIGID_BODY){ - if (!simGripper->m_rigidGrippingConstraints[sIdx]){ + if (!simDev->m_rigidGrippingConstraints[sIdx]){ btRigidBody* bodyAPtr = proximitySensorPtr->getParentBody()->m_bulletRigidBody; btRigidBody* bodyBPtr = proximitySensorPtr->getSensedRigidBody(); if (!rootLink->isChild(bodyBPtr)){ cVector3d hitPointInWorld = proximitySensorPtr->getSensedPoint(); btVector3 pvtA = bodyAPtr->getCenterOfMassTransform().inverse() * toBTvec(hitPointInWorld); btVector3 pvtB = bodyBPtr->getCenterOfMassTransform().inverse() * toBTvec(hitPointInWorld); - simGripper->m_rigidGrippingConstraints[sIdx] = new btPoint2PointConstraint(*bodyAPtr, *bodyBPtr, pvtA, pvtB); - simGripper->m_rigidGrippingConstraints[sIdx]->m_setting.m_impulseClamp = 3.0; - simGripper->m_rigidGrippingConstraints[sIdx]->m_setting.m_tau = 0.001f; - g_bulletWorld->m_bulletWorld->addConstraint(simGripper->m_rigidGrippingConstraints[sIdx]); + simDev->m_rigidGrippingConstraints[sIdx] = new btPoint2PointConstraint(*bodyAPtr, *bodyBPtr, pvtA, pvtB); + simDev->m_rigidGrippingConstraints[sIdx]->m_setting.m_impulseClamp = 3.0; + simDev->m_rigidGrippingConstraints[sIdx]->m_setting.m_tau = 0.001f; + g_bulletWorld->m_bulletWorld->addConstraint(simDev->m_rigidGrippingConstraints[sIdx]); } } } if (proximitySensorPtr->m_sensedBodyType == afProximitySensor::SOFT_BODY){ - if (!simGripper->m_softGrippingConstraints[sIdx]){ + if (!simDev->m_softGrippingConstraints[sIdx]){ // Here we implemented the softBody grad logic. We want to move the // soft body as we move the simulated end effector @@ -1342,9 +1342,9 @@ void updatePhysics(){ // Get the sensed softbody btSoftBody* _sBody = proximitySensorPtr->getSensedSoftBody(); - simGripper->m_softGrippingConstraints[sIdx] = new SoftBodyGrippingConstraint(); - simGripper->m_softGrippingConstraints[sIdx]->m_sBody = _sBody; - simGripper->m_softGrippingConstraints[sIdx]->m_rBody = _rBody; + simDev->m_softGrippingConstraints[sIdx] = new SoftBodyGrippingConstraint(); + simDev->m_softGrippingConstraints[sIdx]->m_sBody = _sBody; + simDev->m_softGrippingConstraints[sIdx]->m_rBody = _rBody; // If we get a sensedSoftBody, we should check if it has a detected face. If a face // is found, we can anchor all the connecting nodes. @@ -1361,7 +1361,7 @@ void updatePhysics(){ _anchor.m_influence = 1; _anchor.m_local = _localPivot; _sBody->m_anchors.push_back(_anchor); - simGripper->m_softGrippingConstraints[sIdx]->m_nodePtrs.push_back(_node); + simDev->m_softGrippingConstraints[sIdx]->m_nodePtrs.push_back(_node); } } // Otherwise we shall directly anchor to nodes. This case @@ -1376,22 +1376,22 @@ void updatePhysics(){ _anchor.m_influence = 1; _anchor.m_local = _localPivot; _sBody->m_anchors.push_back(_anchor); - simGripper->m_softGrippingConstraints[sIdx]->m_nodePtrs.push_back(_node); + simDev->m_softGrippingConstraints[sIdx]->m_nodePtrs.push_back(_node); } } } } else{ - if(simGripper->m_rigidGrippingConstraints[sIdx]){ - g_bulletWorld->m_bulletWorld->removeConstraint(simGripper->m_rigidGrippingConstraints[sIdx]); - simGripper->m_rigidGrippingConstraints[sIdx] = 0; + if(simDev->m_rigidGrippingConstraints[sIdx]){ + g_bulletWorld->m_bulletWorld->removeConstraint(simDev->m_rigidGrippingConstraints[sIdx]); + simDev->m_rigidGrippingConstraints[sIdx] = 0; } - if(simGripper->m_softGrippingConstraints[sIdx]){ - for (int nIdx = 0 ; nIdx < simGripper->m_softGrippingConstraints[sIdx]->m_nodePtrs.size() ; nIdx++){ - btSoftBody::Node* _nodePtr = simGripper->m_softGrippingConstraints[sIdx]->m_nodePtrs[nIdx]; - btSoftBody* _sBody = simGripper->m_softGrippingConstraints[sIdx]->m_sBody; - btRigidBody* _rBody = simGripper->m_softGrippingConstraints[sIdx]->m_rBody; + if(simDev->m_softGrippingConstraints[sIdx]){ + for (int nIdx = 0 ; nIdx < simDev->m_softGrippingConstraints[sIdx]->m_nodePtrs.size() ; nIdx++){ + btSoftBody::Node* _nodePtr = simDev->m_softGrippingConstraints[sIdx]->m_nodePtrs[nIdx]; + btSoftBody* _sBody = simDev->m_softGrippingConstraints[sIdx]->m_sBody; + btRigidBody* _rBody = simDev->m_softGrippingConstraints[sIdx]->m_rBody; for (int aIdx = 0 ; aIdx < _sBody->m_anchors.size() ; aIdx++){ if (_sBody->m_anchors[aIdx].m_body == _rBody){ btSoftBody::Anchor* _anchor = &_sBody->m_anchors[aIdx]; @@ -1402,7 +1402,7 @@ void updatePhysics(){ } } } - simGripper->m_softGrippingConstraints[sIdx] = 0; + simDev->m_softGrippingConstraints[sIdx] = 0; } } } @@ -1411,30 +1411,30 @@ void updatePhysics(){ cVector3d force, torque; - force = rootLink->m_controller.computeOutput(simGripper->m_pos, simGripper->m_posRef, dt); - force = simGripper->P_lc_ramp * force; + force = rootLink->m_controller.computeOutput(simDev->m_pos, simDev->getPosRef(), dt); + force = simDev->P_lc_ramp * force; - torque = rootLink->m_controller.computeOutput(simGripper->m_rot, simGripper->m_rotRef, dt); - simGripper->applyForce(force); - simGripper->applyTorque(torque); - simGripper->setGripperAngle(simGripper->m_gripper_angle, dt); + torque = rootLink->m_controller.computeOutput(simDev->m_rot, simDev->getRotRef(), dt); + simDev->applyForce(force); + simDev->applyTorque(torque); + simDev->setGripperAngle(simDev->m_gripper_angle, dt); - if (simGripper->P_lc_ramp < 1.0) + if (simDev->P_lc_ramp < 1.0) { - simGripper->P_lc_ramp = simGripper->P_lc_ramp + 0.5 * dt; + simDev->P_lc_ramp = simDev->P_lc_ramp + 0.5 * dt; } else { - simGripper->P_lc_ramp = 1.0; + simDev->P_lc_ramp = 1.0; } - if (simGripper->P_ac_ramp < 1.0) + if (simDev->P_ac_ramp < 1.0) { - simGripper->P_ac_ramp = simGripper->P_ac_ramp + 0.5 * dt; + simDev->P_ac_ramp = simDev->P_ac_ramp + 0.5 * dt; } else { - simGripper->P_ac_ramp = 1.0; + simDev->P_ac_ramp = 1.0; } } g_bulletWorld->updateDynamics(dt, g_clockWorld.getCurrentTimeSeconds(), g_freqCounterHaptics.getFrequency(), g_inputDevices->m_numDevices); @@ -1457,19 +1457,19 @@ void updateHapticDevice(void* a_arg){ // update position and orientation of simulated gripper std::string identifyingName = g_inputDevices->m_psDevicePairs[devIdx].m_name; - afPhysicalDevice *pDev = g_inputDevices->m_psDevicePairs[devIdx].m_physicalDevice; - afSimulatedDevice* simGripper = g_inputDevices->m_psDevicePairs[devIdx].m_simulatedDevice; + afPhysicalDevice *phyDev = g_inputDevices->m_psDevicePairs[devIdx].m_physicalDevice; + afSimulatedDevice* simDev = g_inputDevices->m_psDevicePairs[devIdx].m_simulatedDevice; std::vector devCams = g_inputDevices->m_psDevicePairs[devIdx].m_cameras; cLabel* devFreqLabel = g_inputDevices->m_psDevicePairs[devIdx].m_devFreqLabel; if (g_inputDevices->m_psDevicePairs[devIdx].m_cameras.size() == 0){ - cerr << "WARNING: DEVICE HAPTIC LOOP \"" << pDev->m_hInfo.m_modelName << "\" NO WINDOW-CAMERA PAIR SPECIFIED, USING DEFAULT" << endl; + cerr << "WARNING: DEVICE HAPTIC LOOP \"" << phyDev->m_hInfo.m_modelName << "\" NO WINDOW-CAMERA PAIR SPECIFIED, USING DEFAULT" << endl; devCams = g_cameras; } - pDev->m_posClutched.set(0.0,0.0,0.0); - pDev->measuredRot(); - pDev->m_rotClutched.identity(); - simGripper->m_rotRefOrigin = pDev->m_rot; + phyDev->m_posClutched.set(0.0,0.0,0.0); + phyDev->measuredRot(); + phyDev->m_rotClutched.identity(); + simDev->m_rotRefOrigin = phyDev->m_rot; cVector3d dpos, ddpos, dposLast; cMatrix3d drot, ddrot, drotLast; @@ -1484,7 +1484,7 @@ void updateHapticDevice(void* a_arg){ double K_ah_offset = 1; double wait_time = 1.0; - if (std::strcmp(pDev->m_hInfo.m_modelName.c_str(), "Razer Hydra") == 0 ){ + if (std::strcmp(phyDev->m_hInfo.m_modelName.c_str(), "Razer Hydra") == 0 ){ wait_time = 5.0; } @@ -1505,9 +1505,9 @@ void updateHapticDevice(void* a_arg){ // main haptic simulation loop while(g_simulationRunning) { - pDev->m_freq_ctr.signal(1); + phyDev->m_freq_ctr.signal(1); if (devFreqLabel != NULL){ - devFreqLabel->setText(identifyingName + " [" + pDev->m_hInfo.m_modelName + "] " + ": " + cStr(pDev->m_freq_ctr.getFrequency(), 0) + " Hz"); + devFreqLabel->setText(identifyingName + " [" + phyDev->m_hInfo.m_modelName + "] " + ": " + cStr(phyDev->m_freq_ctr.getFrequency(), 0) + " Hz"); } // Adjust time dilation by computing dt from clockWorld time and the simulationTime double dt; @@ -1518,38 +1518,38 @@ void updateHapticDevice(void* a_arg){ else{ dt = compute_dt(); } - pDev->m_pos = pDev->measuredPos(); - pDev->m_rot = pDev->measuredRot(); + phyDev->m_pos = phyDev->measuredPos(); + phyDev->m_rot = phyDev->measuredRot(); - if(pDev->m_gripper_pinch_btn >= 0){ - if(pDev->isButtonPressed(pDev->m_gripper_pinch_btn)){ - pDev->enableForceFeedback(true); + if(phyDev->m_gripper_pinch_btn >= 0){ + if(phyDev->isButtonPressed(phyDev->m_gripper_pinch_btn)){ + phyDev->enableForceFeedback(true); } } - if (pDev->m_hInfo.m_sensedGripper){ - simGripper->m_gripper_angle = pDev->measuredGripperAngle(); + if (phyDev->m_hInfo.m_sensedGripper){ + simDev->m_gripper_angle = phyDev->measuredGripperAngle(); } - else if (pDev->m_buttons.G1 > 0){ + else if (phyDev->m_buttons.G1 > 0){ // Some devices may have a gripper button instead of a continous gripper. - simGripper->m_gripper_angle = !pDev->isButtonPressed(pDev->m_buttons.G1); + simDev->m_gripper_angle = !phyDev->isButtonPressed(phyDev->m_buttons.G1); } else{ - simGripper->m_gripper_angle = 0.5; + simDev->m_gripper_angle = 0.5; } - if(pDev->isButtonPressRisingEdge(pDev->m_buttons.NEXT_MODE)) g_inputDevices->nextMode(); - if(pDev->isButtonPressRisingEdge(pDev->m_buttons.PREV_MODE)) g_inputDevices->prevMode(); + if(phyDev->isButtonPressRisingEdge(phyDev->m_buttons.NEXT_MODE)) g_inputDevices->nextMode(); + if(phyDev->isButtonPressRisingEdge(phyDev->m_buttons.PREV_MODE)) g_inputDevices->prevMode(); - bool btn_1_rising_edge = pDev->isButtonPressRisingEdge(pDev->m_buttons.A1); - bool btn_1_falling_edge = pDev->isButtonPressFallingEdge(pDev->m_buttons.A1); - bool btn_2_rising_edge = pDev->isButtonPressRisingEdge(pDev->m_buttons.A2); - bool btn_2_falling_edge = pDev->isButtonPressFallingEdge(pDev->m_buttons.A2); + bool btn_1_rising_edge = phyDev->isButtonPressRisingEdge(phyDev->m_buttons.A1); + bool btn_1_falling_edge = phyDev->isButtonPressFallingEdge(phyDev->m_buttons.A1); + bool btn_2_rising_edge = phyDev->isButtonPressRisingEdge(phyDev->m_buttons.A2); + bool btn_2_falling_edge = phyDev->isButtonPressFallingEdge(phyDev->m_buttons.A2); double gripper_offset = 0; switch (g_inputDevices->m_simModes){ case MODES::CAM_CLUTCH_CONTROL: - g_inputDevices->g_clutch_btn_pressed = pDev->isButtonPressed(pDev->m_buttons.A1); - g_inputDevices->g_cam_btn_pressed = pDev->isButtonPressed(pDev->m_buttons.A2); + g_inputDevices->g_clutch_btn_pressed = phyDev->isButtonPressed(phyDev->m_buttons.A1); + g_inputDevices->g_cam_btn_pressed = phyDev->isButtonPressed(phyDev->m_buttons.A2); if(g_inputDevices->g_clutch_btn_pressed) g_inputDevices->g_btn_action_str = "Clutch Pressed"; if(g_inputDevices->g_cam_btn_pressed) {g_inputDevices->g_btn_action_str = "Cam Pressed";} if(btn_1_falling_edge || btn_2_falling_edge) g_inputDevices->g_btn_action_str = ""; @@ -1557,7 +1557,7 @@ void updateHapticDevice(void* a_arg){ case MODES::GRIPPER_JAW_CONTROL: if (btn_1_rising_edge) gripper_offset = 0.1; if (btn_2_rising_edge) gripper_offset = -0.1; - simGripper->offsetGripperAngle(gripper_offset); + simDev->offsetGripperAngle(gripper_offset); break; case MODES::CHANGE_CONT_LIN_GAIN: if(btn_1_rising_edge) g_inputDevices->increment_P_lc(P_lc_offset); @@ -1585,78 +1585,78 @@ void updateHapticDevice(void* a_arg){ break; } - pDev->m_hDevice->getUserSwitch(pDev->m_buttons.A2, devCams[0]->m_cam_pressed); + devCams[0]->m_cam_pressed = phyDev->isButtonPressed(phyDev->m_buttons.A2); if(devCams[0]->m_cam_pressed && g_inputDevices->m_simModes == MODES::CAM_CLUTCH_CONTROL){ double scale = 0.01; for (int dcIdx = 0 ; dcIdx < devCams.size() ; dcIdx++){ - devCams[dcIdx]->setLocalPos(devCams[dcIdx]->measuredPos() + cMul(scale, devCams[dcIdx]->measuredRot() * pDev->measuredVelLin() ) ); - devCams[dcIdx]->setLocalRot(pDev->measuredRotCamPreclutch() * cTranspose(pDev->measuredRotPreclutch()) * pDev->measuredRot()); + devCams[dcIdx]->setLocalPos(devCams[dcIdx]->measuredPos() + cMul(scale, devCams[dcIdx]->measuredRot() * phyDev->measuredVelLin() ) ); + devCams[dcIdx]->setLocalRot(phyDev->measuredRotCamPreclutch() * cTranspose(phyDev->measuredRotPreclutch()) * phyDev->measuredRot()); } } if (!devCams[0]->m_cam_pressed){ - pDev->setRotCamPreclutch( devCams[0]->measuredRot() ); - pDev->setRotPreclutch( pDev->measuredRot() ); + phyDev->setRotCamPreclutch( devCams[0]->measuredRot() ); + phyDev->setRotPreclutch( phyDev->measuredRot() ); } if (g_clockWorld.getCurrentTimeSeconds() < wait_time){ - pDev->m_posClutched = pDev->m_pos; + phyDev->m_posClutched = phyDev->m_pos; } if(g_inputDevices->g_cam_btn_pressed){ - if(pDev->btn_cam_rising_edge){ - pDev->btn_cam_rising_edge = false; - simGripper->m_posRefOrigin = simGripper->m_posRef / pDev->m_workspaceScale; - simGripper->m_rotRefOrigin = simGripper->m_rotRef; + if(phyDev->btn_cam_rising_edge){ + phyDev->btn_cam_rising_edge = false; + simDev->m_posRefOrigin = simDev->getPosRef()/ phyDev->m_workspaceScale; + simDev->m_rotRefOrigin = simDev->getRotRef(); } - pDev->m_posClutched = pDev->m_pos; - pDev->m_rotClutched = pDev->m_rot; + phyDev->m_posClutched = phyDev->m_pos; + phyDev->m_rotClutched = phyDev->m_rot; } else{ - pDev->btn_cam_rising_edge = true; + phyDev->btn_cam_rising_edge = true; } if(g_inputDevices->g_clutch_btn_pressed){ - if(pDev->btn_clutch_rising_edge){ - pDev->btn_clutch_rising_edge = false; - simGripper->m_posRefOrigin = simGripper->m_posRef / pDev->m_workspaceScale; - simGripper->m_rotRefOrigin = simGripper->m_rotRef; + if(phyDev->btn_clutch_rising_edge){ + phyDev->btn_clutch_rising_edge = false; + simDev->m_posRefOrigin = simDev->getPosRef() / phyDev->m_workspaceScale; + simDev->m_rotRefOrigin = simDev->getRotRef(); } - pDev->m_posClutched = pDev->m_pos; - pDev->m_rotClutched = pDev->m_rot; + phyDev->m_posClutched = phyDev->m_pos; + phyDev->m_rotClutched = phyDev->m_rot; } else{ - pDev->btn_clutch_rising_edge = true; + phyDev->btn_clutch_rising_edge = true; } - simGripper->m_posRef = pDev->m_workspaceScale * (simGripper->m_posRefOrigin + - (devCams[0]->getLocalRot() * (pDev->m_pos - pDev->m_posClutched))); + simDev->setPosRef(phyDev->m_workspaceScale * (simDev->m_posRefOrigin + + (devCams[0]->getLocalRot() * (phyDev->m_pos - phyDev->m_posClutched)))); if (!g_inputDevices->m_use_cam_frame_rot){ - simGripper->m_rotRef = simGripper->m_rotRefOrigin * devCams[0]->getLocalRot() * - cTranspose(pDev->m_rotClutched) * pDev->m_rot * - cTranspose(devCams[0]->getLocalRot()); + simDev->setRotRef(simDev->m_rotRefOrigin * devCams[0]->getLocalRot() * + cTranspose(phyDev->m_rotClutched) * phyDev->m_rot * + cTranspose(devCams[0]->getLocalRot())); } else{ - simGripper->m_rotRef = pDev->m_simRotInitial * pDev->m_rot * pDev->m_simRotOffset; + simDev->setRotRef(phyDev->m_simRotInitial * phyDev->m_rot * phyDev->m_simRotOffset); } - _refSphere->setLocalPos(simGripper->m_posRef); - _refSphere->setLocalRot(simGripper->m_rotRef); + _refSphere->setLocalPos(simDev->getPosRef()); + _refSphere->setLocalRot(simDev->getRotRef()); // update position of simulated gripper - simGripper->updateMeasuredPose(); + simDev->updateMeasuredPose(); - double P_lin = simGripper->m_rootLink->m_controller.getP_lin(); - double D_lin = simGripper->m_rootLink->m_controller.getD_lin(); - double P_ang = simGripper->m_rootLink->m_controller.getP_ang(); - double D_ang = simGripper->m_rootLink->m_controller.getD_ang(); + double P_lin = simDev->m_rootLink->m_controller.getP_lin(); + double D_lin = simDev->m_rootLink->m_controller.getD_lin(); + double P_ang = simDev->m_rootLink->m_controller.getP_ang(); + double D_ang = simDev->m_rootLink->m_controller.getD_ang(); dposLast = dpos; - dpos = simGripper->m_posRef - simGripper->m_pos; + dpos = simDev->getPosRef() - simDev->m_pos; ddpos = (dpos - dposLast) / dt; drotLast = drot; - drot = cTranspose(simGripper->m_rot) * simGripper->m_rotRef; + drot = cTranspose(simDev->m_rot) * simDev->getRotRef(); ddrot = (cTranspose(drot) * drotLast); double angle, dangle; @@ -1666,29 +1666,29 @@ void updateHapticDevice(void* a_arg){ force_prev = force; torque_prev = torque_prev; - force = - g_cmdOpts.enableForceFeedback * pDev->K_lh_ramp * (P_lin * dpos + D_lin * ddpos); - torque = - g_cmdOpts.enableForceFeedback * pDev->K_ah_ramp * (P_ang * angle * axis); + force = - g_cmdOpts.enableForceFeedback * phyDev->K_lh_ramp * (P_lin * dpos + D_lin * ddpos); + torque = - g_cmdOpts.enableForceFeedback * phyDev->K_ah_ramp * (P_ang * angle * axis); - if ((force - force_prev).length() > pDev->m_maxJerk){ + if ((force - force_prev).length() > phyDev->m_maxJerk){ cVector3d normalized_force = force; normalized_force.normalize(); double _sign = 1.0; if (force.x() < 0 || force.y() < 0 || force.z() < 0){ _sign = 1.0; } - force = force_prev + (normalized_force * pDev->m_maxJerk * _sign); + force = force_prev + (normalized_force * phyDev->m_maxJerk * _sign); } - if (force.length() < pDev->m_deadBand){ + if (force.length() < phyDev->m_deadBand){ force.set(0,0,0); } - if (force.length() > pDev->m_maxForce){ + if (force.length() > phyDev->m_maxForce){ force.normalize(); - force = force * pDev->m_maxForce; + force = force * phyDev->m_maxForce; } - if (torque.length() < pDev->m_deadBand){ + if (torque.length() < phyDev->m_deadBand){ torque.set(0,0,0); } @@ -1696,27 +1696,27 @@ void updateHapticDevice(void* a_arg){ force_msg.push_back(force.x()); force_msg.push_back(force.y()); force_msg.push_back(force.z()); - simGripper->m_rootLink->m_afObjectPtr->set_userdata(force_msg); + simDev->m_rootLink->m_afObjectPtr->set_userdata(force_msg); - pDev->applyWrench(force, torque); + phyDev->applyWrench(force, torque); rateSleep.sleep(); - if (pDev->K_lh_ramp < pDev->K_lh) + if (phyDev->K_lh_ramp < phyDev->K_lh) { - pDev->K_lh_ramp = pDev->K_lh_ramp + 0.1 * dt * pDev->K_lh; + phyDev->K_lh_ramp = phyDev->K_lh_ramp + 0.1 * dt * phyDev->K_lh; } else { - pDev->K_lh_ramp = pDev->K_lh; + phyDev->K_lh_ramp = phyDev->K_lh; } - if (pDev->K_ah_ramp < pDev->K_ah) + if (phyDev->K_ah_ramp < phyDev->K_ah) { - pDev->K_ah_ramp = pDev->K_ah_ramp + 0.1 * dt * pDev->K_ah; + phyDev->K_ah_ramp = phyDev->K_ah_ramp + 0.1 * dt * phyDev->K_ah; } else { - pDev->K_ah_ramp = pDev->K_ah; + phyDev->K_ah_ramp = phyDev->K_ah; } } From 8f74246c4b6423d95f692c78a553811a5c1e53fb Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Mon, 2 Sep 2019 20:35:34 -0400 Subject: [PATCH 3/6] Update README.md --- README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/README.md b/README.md index eff860619..f9629da01 100644 --- a/README.md +++ b/README.md @@ -176,3 +176,13 @@ _client.clean_up() ### The AMBF Controller Client See [here](/ambf_controller/README.md) for more information. + +## Citation +If this work is helpful for your research, please use the following reference for citation: +``` +Munawar, Adnan, Wang, Yan, Gondokaryono, Radian & Gregory Fischer. "A Real-Time Dynamic Simulator and +an Associated Front-End Representation Format for Simulating Complex Robots and Environments" 2019 +IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019 +``` + + From e4e7d40a2856cd085851a238554b253e9fb82106 Mon Sep 17 00:00:00 2001 From: nathaniel Date: Mon, 2 Sep 2019 21:22:16 -0400 Subject: [PATCH 4/6] removed redudant code for initilization of arrayies --- ambf_ros_modules/ambf_comm/scripts/ambf_object.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/ambf_ros_modules/ambf_comm/scripts/ambf_object.py b/ambf_ros_modules/ambf_comm/scripts/ambf_object.py index dad3fbb2d..83f02634e 100755 --- a/ambf_ros_modules/ambf_comm/scripts/ambf_object.py +++ b/ambf_ros_modules/ambf_comm/scripts/ambf_object.py @@ -329,11 +329,8 @@ def set_joint_pos(self, joint, pos): return if len(self._cmd.joint_cmds) != n_jnts: - self._cmd.joint_cmds = [0]*n_jnts - for j_idx in range(0, n_jnts): - self._cmd.joint_cmds[j_idx] = 0.0 - self._cmd.position_controller_mask = [0]*n_jnts - + self._cmd.joint_cmds = [0.0]*n_jnts + self._cmd.joint_cmds[idx] = pos self._cmd.position_controller_mask[idx] = True @@ -399,10 +396,7 @@ def set_joint_effort(self, joint, effort): return if len(self._cmd.joint_cmds) != n_jnts: - self._cmd.joint_cmds = [0] * n_jnts - for j_idx in range(0, n_jnts): - self._cmd.joint_cmds[j_idx] = 0.0 - self._cmd.position_controller_mask = [0]*n_jnts + self._cmd.joint_cmds = [0.0] * n_jnts self._cmd.joint_cmds[idx] = effort self._cmd.position_controller_mask[idx] = False From d1c5c8d3b50a2dc084b5eff3e8d4ccdbc0f7abf2 Mon Sep 17 00:00:00 2001 From: nathaniel Date: Tue, 3 Sep 2019 12:52:03 -0400 Subject: [PATCH 5/6] add postion control mask initlitization --- ambf_ros_modules/ambf_comm/scripts/ambf_object.py | 1 + 1 file changed, 1 insertion(+) diff --git a/ambf_ros_modules/ambf_comm/scripts/ambf_object.py b/ambf_ros_modules/ambf_comm/scripts/ambf_object.py index 83f02634e..23dd74e07 100755 --- a/ambf_ros_modules/ambf_comm/scripts/ambf_object.py +++ b/ambf_ros_modules/ambf_comm/scripts/ambf_object.py @@ -398,6 +398,7 @@ def set_joint_effort(self, joint, effort): if len(self._cmd.joint_cmds) != n_jnts: self._cmd.joint_cmds = [0.0] * n_jnts + self._cmd.position_controller_mask = [0]*n_jnts self._cmd.joint_cmds[idx] = effort self._cmd.position_controller_mask[idx] = False From 28aa798e762042fbe26b3aac16da34c07874fd05 Mon Sep 17 00:00:00 2001 From: nathaniel Date: Tue, 3 Sep 2019 16:49:27 -0400 Subject: [PATCH 6/6] moved line to set mask inside conditional --- ambf_ros_modules/ambf_comm/scripts/ambf_object.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ambf_ros_modules/ambf_comm/scripts/ambf_object.py b/ambf_ros_modules/ambf_comm/scripts/ambf_object.py index 23dd74e07..8044fb56f 100755 --- a/ambf_ros_modules/ambf_comm/scripts/ambf_object.py +++ b/ambf_ros_modules/ambf_comm/scripts/ambf_object.py @@ -330,6 +330,7 @@ def set_joint_pos(self, joint, pos): if len(self._cmd.joint_cmds) != n_jnts: self._cmd.joint_cmds = [0.0]*n_jnts + self._cmd.position_controller_mask = [1]*n_jnts self._cmd.joint_cmds[idx] = pos self._cmd.position_controller_mask[idx] = True @@ -397,8 +398,8 @@ def set_joint_effort(self, joint, effort): if len(self._cmd.joint_cmds) != n_jnts: self._cmd.joint_cmds = [0.0] * n_jnts + self._cmd.position_controller_mask = [0]*n_jnts - self._cmd.position_controller_mask = [0]*n_jnts self._cmd.joint_cmds[idx] = effort self._cmd.position_controller_mask[idx] = False