Allow ref_name to be passed into OSC instead of always 'EE'

- Also moved the code to set the 'hand' object equal to the 'EE'
  orientation and position into a separate function that has to be
  called, intsead of it happening by default when calling send_forces
    - this allows models that don't have an 'EE' object to be controlled
- Now the ref_frame parameter specifies what object is used as the
  taskspace when the ctrl.generate function is called for OSC

abr_control/controllers/osc.py

@@ -146,14 +146,14 @@ def _Mx(self, M, J, threshold=1e-3):
 
         return Mx, M_inv
 
-    def _calc_orientation_forces(self, target_abg, q):
+    def _calc_orientation_forces(self, target_abg, q, ref_frame):
         """Calculate the desired Euler angle forces to apply to the arm to
         move the end-effector to the target orientation
 
         Parameters
         ----------
         target_abg: np.array
-            the target Euler angles orientation for the end-effector:
+            the target Euler angles orientation for the ref_frame object:
             alpha, beta, gamma
         q: np.array
             the joint angles of the arm
@@ -167,7 +167,7 @@ def _calc_orientation_forces(self, target_abg, q):
                 )
             )
             # get the quaternion for the end effector
-            q_e = self.robot_config.quaternion("EE", q=q)
+            q_e = self.robot_config.quaternion(ref_frame, q=q)
             q_r = transformations.quaternion_multiply(
                 q_d, transformations.quaternion_conjugate(q_e)
             )
@@ -176,7 +176,7 @@ def _calc_orientation_forces(self, target_abg, q):
         elif self.orientation_algorithm == 1:
             # From (Caccavale et al, 1997) Section IV Quaternion feedback
             # get rotation matrix for the end effector orientation
-            R_e = self.robot_config.R("EE", q)
+            R_e = self.robot_config.R(ref_frame, q)
             # get rotation matrix for the target orientation
             R_d = transformations.euler_matrix(
                 target_abg[0], target_abg[1], target_abg[2], axes="rxyz"
@@ -256,7 +256,7 @@ def generate(
 
         # if orientation is being controlled
         if np.sum(self.ctrlr_dof[3:]) > 0:
-            u_task[3:] = self._calc_orientation_forces(target[3:], q)
+            u_task[3:] = self._calc_orientation_forces(target[3:], q, ref_frame=ref_frame)
 
         # task space integrated error term
         if self.ki != 0:

abr_control/interfaces/mujoco.py

@@ -172,12 +172,8 @@ def get_joint_vel_addrs(self, jntadr):
 
     def get_joint_dyn_addrs(self, jntadr):
         # store the data.ctrl indices associated with this joint
-        print(f"{jntadr=}")
         for first_dyn, v in enumerate(self.model.actuator_trnid):
-            print(f"{first_dyn=}")
-            print(f"{v=}")
             if v[0] == jntadr:
-                print(f"v[0] == jntadr")
                 break
         dynvec_length = self.robot_config.JNT_DYN_LENGTH[self.model.jnt_type[jntadr]]
         joint_dyn_addr = list(range(first_dyn, first_dyn + dynvec_length))[::-1]
@@ -363,6 +359,28 @@ def set_mocap_orientation(self, name, quat):
         self.data.mocap_quat[mocap_id] = quat
         mujoco.mj_forward(self.model, self.data)
 
+    def set_mocap_state_equal_to(self, mocap_name, object_name, q=None):
+        """Matches the position and orientation of an object in the Mujoco environment
+        to another object in the environment
+
+        Parameters
+        ----------
+        mocap_name: string
+            the name of the mocap_object to move
+        object_name: string
+            the name of the object to match
+        q: np.array
+            configuration for reading object position and orientation [radians]
+        """
+
+        # Update position
+        object_xyz = self.robot_config.Tx(name=object_name, q=q)
+        self.set_mocap_xyz(mocap_name, object_xyz)
+
+        # Update orientation of hand object
+        object_quat = self.robot_config.quaternion(name=object_name, q=q)
+        self.set_mocap_orientation(mocap_name, object_quat)
+
     def set_state(self, name, xyz=None, quat=None):
         """Sets the state of an object attached to the world with a free joint.