Fixing joint cmd array initilization

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
+```
+
+

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<std::mutex> lock(m_mutex);
+    m_posRef = a_pos;
+}
+
+
+///
+/// \brief afSharedDataStructure::setRotRef
+/// \param a_rot
+///
+void afSharedDataStructure::setRotRef(cMatrix3d a_rot){
+    std::lock_guard<std::mutex> lock(m_mutex);
+    m_rotRef = a_rot;
+}
+
+
+///
+/// \brief afSharedDataStructure::getPosRef
+/// \return
+///
+cVector3d afSharedDataStructure::getPosRef(){
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_posRef;
+}
+
+///
+/// \brief afSharedDataStructure::getRotRef
+/// \return
+///
+cMatrix3d afSharedDataStructure::getRotRef(){
+    std::lock_guard<std::mutex> 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;
 

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

ambf_models/descriptions/launch.yaml

@@ -23,3 +23,4 @@ multibody configs:
   # - "./multi-bodies/puzzles/puzzle5.yaml" #16
   # - "./multi-bodies/puzzles/puzzle6.yaml" #17
   - "/home/nathaniel/Dropbox/Hex Exo Skeleton/ambf model/hex2.yaml"
+

ambf_ros_modules/ambf_comm/scripts/ambf_object.py

@@ -391,11 +391,9 @@ 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.position_controller_mask = [1]*n_jnts
+
         self._cmd.joint_cmds[idx] = pos
         self._cmd.position_controller_mask[idx] = True
 
@@ -460,8 +458,8 @@ def set_joint_effort(self, joint, effort):
             return
 
         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 = [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