Returns the current cartesian pose of the last joint of the ik robot with respect to the base frame as
        a (pos, orn) tuple where orn is a x-y-z-w quaternion
544987
def ik_robot_eef_joint_cartesian_pose(self):
        
        eef_pos_in_world = np.array(p.getLinkState(self.ik_robot, 6)[0])
        eef_orn_in_world = np.array(p.getLinkState(self.ik_robot, 6)[1])
        eef_pose_in_world = T.pose2mat((eef_pos_in_world, eef_orn_in_world))

        base_pos_in_world = np.array(p.getBasePositionAndOrientation(self.ik_robot)[0])
        base_orn_in_world = np.array(p.getBasePositionAndOrientation(self.ik_robot)[1])
        base_pose_in_world = T.pose2mat((base_pos_in_world, base_orn_in_world))
        world_pose_in_base = T.pose_inv(base_pose_in_world)

        eef_pose_in_base = T.pose_in_A_to_pose_in_B(
            pose_A=eef_pose_in_world, pose_A_in_B=world_pose_in_base
        )

        return T.mat2pose(eef_pose_in_base)
591320
def transform_vector_coorb_to_inertial(vec_coorb, orbPhase, quat_copr):
    

    # Transform to coprecessing frame
    vec_copr = rotate_in_plane(vec_coorb, -orbPhase)

    # Transform to inertial frame
    vec = transformTimeDependentVector(np.array([quat_copr]).T,
        np.array([vec_copr]).T).T[0]

    return np.array(vec)
7605
def _get_observation(self):
        
        di = super()._get_observation()
        # proprioceptive features
        di["joint_pos"] = np.array(
            [self.sim.data.qpos[x] for x in self._ref_joint_pos_indexes]
        )
        di["joint_vel"] = np.array(
            [self.sim.data.qvel[x] for x in self._ref_joint_vel_indexes]
        )
        robot_states = [
            np.sin(di["joint_pos"]),
            np.cos(di["joint_pos"]),
            di["joint_vel"],
        ]

        if self.has_gripper_right:
            di["right_gripper_qpos"] = np.array(
                [
                    self.sim.data.qpos[x]
                    for x in self._ref_gripper_right_joint_pos_indexes
                ]
            )
            di["right_gripper_qvel"] = np.array(
                [
                    self.sim.data.qvel[x]
                    for x in self._ref_gripper_right_joint_vel_indexes
                ]
            )
            di["right_eef_pos"] = self.sim.data.site_xpos[self.right_eef_site_id]
            di["right_eef_quat"] = T.convert_quat(
                self.sim.data.get_body_xquat("right_hand"), to="xyzw"
            )
            robot_states.extend(
                [di["right_gripper_qpos"], di["right_eef_pos"], di["right_eef_quat"]]
            )

        if self.has_gripper_left:
            di["left_gripper_qpos"] = np.array(
                [
                    self.sim.data.qpos[x]
                    for x in self._ref_gripper_left_joint_pos_indexes
                ]
            )
            di["left_gripper_qvel"] = np.array(
                [
                    self.sim.data.qvel[x]
                    for x in self._ref_gripper_left_joint_vel_indexes
                ]
            )
            di["left_eef_pos"] = self.sim.data.site_xpos[self.left_eef_site_id]
            di["left_eef_quat"] = T.convert_quat(
                self.sim.data.get_body_xquat("left_hand"), to="xyzw"
            )
            robot_states.extend(
                [di["left_gripper_qpos"], di["left_eef_pos"], di["left_eef_quat"]]
            )

        di["robot-state"] = np.concatenate(robot_states)
        return di
497734
def _get_reference(self):
        """"""
        super()._get_reference()

        # indices for joints in qpos, qvel
        self.robot_joints = list(self.mujoco_robot.joints)
        self._ref_joint_pos_indexes = [
            self.sim.model.get_joint_qpos_addr(x) for x in self.robot_joints
        ]
        self._ref_joint_vel_indexes = [
            self.sim.model.get_joint_qvel_addr(x) for x in self.robot_joints
        ]
        if self.use_indicator_object:
            ind_qpos = self.sim.model.get_joint_qpos_addr("pos_indicator")
            self._ref_indicator_pos_low, self._ref_indicator_pos_high = ind_qpos

            ind_qvel = self.sim.model.get_joint_qvel_addr("pos_indicator")
            self._ref_indicator_vel_low, self._ref_indicator_vel_high = ind_qvel

            self.indicator_id = self.sim.model.body_name2id("pos_indicator")

        # indices for grippers in qpos, qvel
        if self.has_gripper_left:
            self.gripper_left_joints = list(self.gripper_left.joints)
            self._ref_gripper_left_joint_pos_indexes = [
                self.sim.model.get_joint_qpos_addr(x) for x in self.gripper_left_joints
            ]
            self._ref_gripper_left_joint_vel_indexes = [
                self.sim.model.get_joint_qvel_addr(x) for x in self.gripper_left_joints
            ]
            self.left_eef_site_id = self.sim.model.site_name2id("l_g_grip_site")

        if self.has_gripper_right:
            self.gripper_right_joints = list(self.gripper_right.joints)
            self._ref_gripper_right_joint_pos_indexes = [
                self.sim.model.get_joint_qpos_addr(x) for x in self.gripper_right_joints
            ]
            self._ref_gripper_right_joint_vel_indexes = [
                self.sim.model.get_joint_qvel_addr(x) for x in self.gripper_right_joints
            ]
            self.right_eef_site_id = self.sim.model.site_name2id("grip_site")

        # indices for joint pos actuation, joint vel actuation, gripper actuation
        self._ref_joint_pos_actuator_indexes = [
            self.sim.model.actuator_name2id(actuator)
            for actuator in self.sim.model.actuator_names
            if actuator.startswith("pos")
        ]

        self._ref_joint_vel_actuator_indexes = [
            self.sim.model.actuator_name2id(actuator)
            for actuator in self.sim.model.actuator_names
            if actuator.startswith("vel")
        ]

        if self.has_gripper_left:
            self._ref_joint_gripper_left_actuator_indexes = [
                self.sim.model.actuator_name2id(actuator)
                for actuator in self.sim.model.actuator_names
                if actuator.startswith("gripper_l")
            ]

        if self.has_gripper_right:
            self._ref_joint_gripper_right_actuator_indexes = [
                self.sim.model.actuator_name2id(actuator)
                for actuator in self.sim.model.actuator_names
                if actuator.startswith("gripper_r")
            ]

        if self.has_gripper_right:
            # IDs of sites for gripper visualization
            self.eef_site_id = self.sim.model.site_name2id("grip_site")
            self.eef_cylinder_id = self.sim.model.site_name2id("grip_site_cylinder")
1200458
def get_symmetry_operations(self, cartesian=False):
        
        rotation, translation = self._get_symmetry()
        symmops = []
        mat = self._structure.lattice.matrix.T
        invmat = np.linalg.inv(mat)
        for rot, trans in zip(rotation, translation):
            if cartesian:
                rot = np.dot(mat, np.dot(rot, invmat))
                trans = np.dot(trans, self._structure.lattice.matrix)
            op = SymmOp.from_rotation_and_translation(rot, trans)
            symmops.append(op)
        return symmops
767049
def from_ros_pose_msg(pose_msg,
                          from_frame='unassigned',
                          to_frame='world'):
        
        quaternion = np.array([pose_msg.orientation.w,
                               pose_msg.orientation.x,
                               pose_msg.orientation.y,
                               pose_msg.orientation.z])
        position = np.array([pose_msg.position.x,
                             pose_msg.position.y,
                             pose_msg.position.z])
        pose = RigidTransform(rotation=quaternion,
                              translation=position,
                              from_frame=from_frame,
                              to_frame=to_frame)
        return pose
278718
function() {
        var ox = this._origin.x + this._x,
            oy = this._origin.y + this._y,
            rad = -this._rotation * DEG_TO_RAD;
        // axis-aligned (unrotated) coordinates, relative to the origin point
        var dx1 = this._x - this._bx1 - ox,
            dx2 = this._x + this._w + this._bx2 - ox,
            dy1 = this._y - this._by1 - oy,
            dy2 = this._y + this._h + this._by2 - oy;

        var ct = rounded_cos(rad),
            st = rounded_sin(rad);

        // Calculate the new points relative to the origin, then find the new (absolute) bounding coordinates!
        var x0 = dx1 * ct + dy1 * st,
            y0 = -dx1 * st + dy1 * ct,
            x1 = dx2 * ct + dy1 * st,
            y1 = -dx2 * st + dy1 * ct,
            x2 = dx2 * ct + dy2 * st,
            y2 = -dx2 * st + dy2 * ct,
            x3 = dx1 * ct + dy2 * st,
            y3 = -dx1 * st + dy2 * ct,
            minx = Math.floor(Math.min(x0, x1, x2, x3) + ox),
            miny = Math.floor(Math.min(y0, y1, y2, y3) + oy),
            maxx = Math.ceil(Math.max(x0, x1, x2, x3) + ox),
            maxy = Math.ceil(Math.max(y0, y1, y2, y3) + oy);
        if (!this._mbr) {
            this._mbr = {
                _x: minx,
                _y: miny,
                _w: maxx - minx,
                _h: maxy - miny
            };
        } else {
            this._mbr._x = minx;
            this._mbr._y = miny;
            this._mbr._w = maxx - minx;
            this._mbr._h = maxy - miny;
        }

        // If a collision hitbox exists AND sits outside the entity, find a bounding box for both.
        // `_cbr` contains information about a bounding circle of the hitbox.
        // The bounds of `_cbr` will be the union of the `_mbr` and the bounding box of that circle.
        // This will not be a minimal region, but since it's only used for the broad phase pass it's good enough.
        //
        // cbr is calculated by the `_checkBounds` method of the "Collision" component
        if (this._cbr) {
            var cbr = this._cbr;
            var cx = cbr.cx,
                cy = cbr.cy,
                r = cbr.r;
            var cx2 = ox + (cx + this._x - ox) * ct + (cy + this._y - oy) * st;
            var cy2 = oy - (cx + this._x - ox) * st + (cy + this._y - oy) * ct;
            cbr._x = Math.min(cx2 - r, minx);
            cbr._y = Math.min(cy2 - r, miny);
            cbr._w = Math.max(cx2 + r, maxx) - cbr._x;
            cbr._h = Math.max(cy2 + r, maxy) - cbr._y;
        }
    }
268004
def computePointing(self, ra_deg, dec_deg, roll_deg, cartesian=False):
        """"""
        # Roll FOV
        Rrotate = r.rotateInXMat(roll_deg)  # Roll

        # Slew from ra/dec of zero
        Ra = r.rightAscensionRotationMatrix(ra_deg)
        Rd = r.declinationRotationMatrix(dec_deg)
        Rslew = np.dot(Ra, Rd)

        R = np.dot(Rslew, Rrotate)

        slew = self.origin*1
        for i, row in enumerate(self.origin):
            slew[i, 3:6] = np.dot(R, row[3:6])

        if cartesian is False:
            slew = self.getRaDecs(slew)
        return slew
90882
def normalized(self):
    

    magnitude = self.magnitude()
    return Quaternion(
      self.x / magnitude, self.y / magnitude,
      self.z / magnitude, self.w / magnitude)
190706
def coordinate(i, j)
      x = @pos.x + (i * @delta_col * @cosines[0]) + (j * @delta_row * @cosines[3])
      y = @pos.y + (i * @delta_col * @cosines[1]) + (j * @delta_row * @cosines[4])
      z = @pos.z + (i * @delta_col * @cosines[2]) + (j * @delta_row * @cosines[5])
      Coordinate.new(x, y, z)
    end
1153831
def _get_reference(self):
        
        super()._get_reference()

        # indices for joints in qpos, qvel
        self.robot_joints = list(self.mujoco_robot.joints)
        self._ref_joint_pos_indexes = [
            self.sim.model.get_joint_qpos_addr(x) for x in self.robot_joints
        ]
        self._ref_joint_vel_indexes = [
            self.sim.model.get_joint_qvel_addr(x) for x in self.robot_joints
        ]

        if self.use_indicator_object:
            ind_qpos = self.sim.model.get_joint_qpos_addr("pos_indicator")
            self._ref_indicator_pos_low, self._ref_indicator_pos_high = ind_qpos

            ind_qvel = self.sim.model.get_joint_qvel_addr("pos_indicator")
            self._ref_indicator_vel_low, self._ref_indicator_vel_high = ind_qvel

            self.indicator_id = self.sim.model.body_name2id("pos_indicator")

        # indices for grippers in qpos, qvel
        if self.has_gripper:
            self.gripper_joints = list(self.gripper.joints)
            self._ref_gripper_joint_pos_indexes = [
                self.sim.model.get_joint_qpos_addr(x) for x in self.gripper_joints
            ]
            self._ref_gripper_joint_vel_indexes = [
                self.sim.model.get_joint_qvel_addr(x) for x in self.gripper_joints
            ]

        # indices for joint pos actuation, joint vel actuation, gripper actuation
        self._ref_joint_pos_actuator_indexes = [
            self.sim.model.actuator_name2id(actuator)
            for actuator in self.sim.model.actuator_names
            if actuator.startswith("pos")
        ]

        self._ref_joint_vel_actuator_indexes = [
            self.sim.model.actuator_name2id(actuator)
            for actuator in self.sim.model.actuator_names
            if actuator.startswith("vel")
        ]

        if self.has_gripper:
            self._ref_joint_gripper_actuator_indexes = [
                self.sim.model.actuator_name2id(actuator)
                for actuator in self.sim.model.actuator_names
                if actuator.startswith("gripper")
            ]

        # IDs of sites for gripper visualization
        self.eef_site_id = self.sim.model.site_name2id("grip_site")
        self.eef_cylinder_id = self.sim.model.site_name2id("grip_site_cylinder")
1203847
def coords_for_computations(self):
        

        # TODO: need to subtract the position offset if a Mesh (in orbit)
        if self._compute_at_vertices:
            if self.pvertices is not None:
                return self.pvertices - self._pos_center
            else:
                return self.vertices - self._pos_center
        else:
            return self.centers - self._pos_center
382194
def inverse(self):
        
        inv_rotation = self.rotation.T
        inv_translation = np.dot(-self.rotation.T, self.translation)
        return RigidTransform(inv_rotation, inv_translation,
                              from_frame=self._to_frame,
                              to_frame=self._from_frame)
522723
def rotate_quat(attitude, roll, pitch, yaw):
  '''
  
  '''
  quat = Quaternion(attitude)
  rotation = Quaternion([roll, pitch, yaw])
  res = rotation * quat

  return res.q
394733
def copy(self):
        
        return RigidTransform(np.copy(self.rotation), np.copy(self.translation), self.from_frame, self.to_frame)
179152
def _spherical_to_cartesian(cls, coord, center):
        
        r, theta, phi, r_dot, theta_dot, phi_dot = coord
        x = r * cos(phi) * cos(theta)
        y = r * cos(phi) * sin(theta)
        z = r * sin(phi)

        vx = r_dot * x / r - y * theta_dot - z * phi_dot * cos(theta)
        vy = r_dot * y / r + x * theta_dot - z * phi_dot * sin(theta)
        vz = r_dot * z / r + r * phi_dot * cos(phi)

        return np.array([x, y, z, vx, vy, vz], dtype=float)
1347763
def convert_from_ik_angles(self, joints):
        
        if len(joints) != len(self.motors) + 2:
            raise ValueError('Incompatible data, len(joints) should be {}!'.format(len(self.motors) + 2))

        joints = [rad2deg(j) for j in joints[1:-1]]
        joints *= self._reversed

        return [(j * (1 if m.direct else -1)) - m.offset
                for j, m in zip(joints, self.motors)]