Switch to a server/client architecture

CMakeLists.txt

@@ -1,4 +1,4 @@
 cmake_minimum_required(VERSION 3.1)
-project(vgn)
+project(active_grasp)
 
 find_package(catkin REQUIRED)

controllers.py

@@ -1,29 +0,0 @@
-import numpy as np
-
-
-class CartesianPoseController:
-    def __init__(self, robot, model, x0, rate):
-        self.robot = robot
-        self.model = model
-        self.rate = rate
-        self.x_d = x0
-        self.kp = np.ones(6) * 4.0
-
-    def set_target(self, pose):
-        self.x_d = pose
-
-    def update(self):
-        q, _ = self.robot.get_state()
-
-        x = self.model.pose(q)
-        x_d = self.x_d
-
-        err = np.zeros(6)
-        err[:3] = x_d.translation - x.translation
-        err[3:] = (x_d.rotation * x.rotation.inv()).as_rotvec()
-
-        J = self.model.jacobian(q)
-        J_pinv = np.linalg.pinv(J)
-        cmd = np.dot(J_pinv, self.kp * err)
-
-        self.robot.set_desired_joint_velocities(cmd)

launch/panda_visualization.launch

@@ -1,7 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
   <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
-
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
   <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" />
 </launch>

launch/panda_visualization.rviz

@@ -5,8 +5,6 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /RobotModel1/Status1
-        - /TF1/Frames1
       Splitter Ratio: 0.5
     Tree Height: 549
   - Class: rviz/Selection
@@ -130,10 +128,34 @@ Visualization Manager:
       Value: true
       Visual Enabled: true
     - Class: rviz/TF
-      Enabled: false
+      Enabled: true
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        panda_hand:
+          Value: true
+        panda_leftfinger:
+          Value: false
+        panda_link0:
+          Value: true
+        panda_link1:
+          Value: false
+        panda_link2:
+          Value: false
+        panda_link3:
+          Value: false
+        panda_link4:
+          Value: false
+        panda_link5:
+          Value: false
+        panda_link6:
+          Value: false
+        panda_link7:
+          Value: false
+        panda_link8:
+          Value: false
+        panda_rightfinger:
+          Value: false
       Marker Alpha: 1
       Marker Scale: 0.5
       Name: TF
@@ -141,17 +163,21 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        panda_link0:
+          panda_link1:
+            panda_link2:
+              panda_link3:
+                panda_link4:
+                  panda_link5:
+                    panda_link6:
+                      panda_link7:
+                        panda_link8:
+                          panda_hand:
+                            panda_leftfinger:
+                              {}
+                            panda_rightfinger:
+                              {}
       Update Interval: 0
-      Value: false
-    - Class: rviz/InteractiveMarkers
-      Enable Transparency: true
-      Enabled: true
-      Name: Target
-      Show Axes: false
-      Show Descriptions: true
-      Show Visual Aids: false
-      Update Topic: /target/update
       Value: true
   Enabled: true
   Global Options:
@@ -181,7 +207,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.2702323198318481
+      Distance: 1.3619381189346313
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -189,17 +215,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.06870806962251663
+        X: 0.14782209694385529
-        Y: 0.16815021634101868
+        Y: 0.10340728610754013
-        Z: 0.3779522478580475
+        Z: 0.3605891168117523
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.38539746403694153
+      Pitch: 0.21979668736457825
       Target Frame: <Fixed Frame>
-      Yaw: 5.683584213256836
+      Yaw: 5.333564758300781
     Saved: ~
 Window Geometry:
   Displays:
@@ -217,5 +243,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1200
-  X: 1045
+  X: 659
-  Y: 249
+  Y: 27

model.py

@@ -1,38 +0,0 @@
-import numpy as np
-import kdl_parser_py.urdf as kdl_parser
-import PyKDL as kdl
-
-import utils
-
-
-class Model(object):
-    def __init__(self, root_frame_id, tip_frame_id):
-        _, tree = kdl_parser.treeFromFile("assets/urdfs/panda/panda_arm_hand.urdf")
-        chain = tree.getChain(root_frame_id, tip_frame_id)
-        self.nr_joints = chain.getNrOfJoints()
-        self.fk_pos_solver = kdl.ChainFkSolverPos_recursive(chain)
-        self.fk_vel_solver = kdl.ChainFkSolverVel_recursive(chain)
-        self.jac_solver = kdl.ChainJntToJacSolver(chain)
-        return
-
-    def pose(self, q):
-        jnt_array = utils.to_kdl_jnt_array(q)
-        frame = kdl.Frame()
-        self.fk_pos_solver.JntToCart(jnt_array, frame)
-        return utils.Transform.from_kdl(frame)
-
-    def velocities(self, q, dq):
-        jnt_array_vel = kdl.JntArrayVel(
-            utils.to_kdl_jnt_array(q), utils.to_kdl_jnt_array(dq)
-        )
-        twist = kdl.FrameVel()
-        self.fk_vel_solver.JntToCart(jnt_array_vel, twist)
-        d = twist.deriv()
-        linear, angular = np.r_[d[0], d[1], d[2]], np.r_[d[3], d[4], d[5]]
-        return linear, angular
-
-    def jacobian(self, q):
-        jnt_array = utils.to_kdl_jnt_array(q)
-        J = kdl.Jacobian(self.nr_joints)
-        self.jac_solver.JntToJac(jnt_array, J)
-        return utils.kdl_to_mat(J)

policies.py

@@ -0,0 +1,46 @@
+from geometry_msgs.msg import Pose
+import numpy as np
+import rospy
+import scipy.interpolate
+
+from robot_tools.ros import *
+
+
+def get_policy(name):
+    if name == "fixed":
+        return FixedTrajectory()
+
+
+class BasePolicy:
+    def __init__(self):
+        self.tf_tree = TransformTree()
+        self.target_pose_pub = rospy.Publisher("/target", Pose, queue_size=10)
+        rospy.sleep(1.0)
+
+
+class FixedTrajectory(BasePolicy):
+    def __init__(self):
+        super().__init__()
+        self.duration = 4.0
+        self.radius = 0.1
+        self.m = scipy.interpolate.interp1d([0, self.duration], [np.pi, 3.0 * np.pi])
+
+    def start(self):
+        self.tic = rospy.Time.now()
+        timeout = rospy.Duration(0.1)
+        x0 = self.tf_tree.lookup("panda_link0", "panda_hand", self.tic, timeout)
+        self.origin = np.r_[x0.translation[0] + self.radius, x0.translation[1:]]
+        self.target = x0
+
+    def update(self):
+        elapsed_time = (rospy.Time.now() - self.tic).to_sec()
+
+        if elapsed_time > self.duration:
+            return True
+
+        t = self.m(elapsed_time)
+        self.target.translation = (
+            self.origin + np.r_[self.radius * np.cos(t), self.radius * np.sin(t), 0.0]
+        )
+        self.target_pose_pub.publish(to_pose_msg(self.target))
+        return False

requirements.txt

@@ -1,3 +0,0 @@
-numpy
-scipy
-pybullet

robot_sim.py

@@ -1,161 +0,0 @@
-import time
-
-import numpy as np
-import pybullet as p
-import pybullet_data
-
-from sensor_msgs.msg import JointState
-
-from model import *
-from utils import *
-
-
-class PandaArm:
-    def __init__(self):
-        self.nr_dof = 7
-
-        self.named_joint_values = {"ready": [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]}
-
-        self.upper_limits = [-7] * self.nr_dof
-        self.lower_limits = [7] * self.nr_dof
-        self.ranges = [7] * self.nr_dof
-
-    def load(self, pose):
-        self.T_world_base = pose
-        self.uid = p.loadURDF(
-            "assets/urdfs/panda/panda_arm_hand.urdf",
-            basePosition=pose.translation,
-            baseOrientation=pose.rotation.as_quat(),
-            useFixedBase=True,
-        )
-        for i, q_i in enumerate(self.named_joint_values["ready"]):
-            p.resetJointState(self.uid, i, q_i)
-
-    def get_state(self):
-        joint_states = p.getJointStates(self.uid, range(p.getNumJoints(self.uid)))[:7]
-        positions = [state[0] for state in joint_states]
-        velocities = [state[1] for state in joint_states]
-        return positions, velocities
-
-    def set_desired_joint_positions(self, positions):
-        for i, position in enumerate(positions):
-            p.setJointMotorControl2(self.uid, i, p.POSITION_CONTROL, position)
-
-    def set_desired_joint_velocities(self, velocities):
-        for i, velocity in enumerate(velocities):
-            p.setJointMotorControl2(
-                self.uid, i, p.VELOCITY_CONTROL, targetVelocity=velocity
-            )
-
-
-class PandaGripper:
-    def move(self, width):
-        for i in [9, 10]:
-            p.setJointMotorControl2(
-                self.uid,
-                i,
-                p.POSITION_CONTROL,
-                0.5 * width,
-                force=10,
-            )
-
-    def read(self):
-        return p.getJointState(self.uid, 9)[0] + p.getJointState(self.uid, 10)[0]
-
-
-class Camera:
-    def __init__(self, uid, link_id, width, height, hfov, near, far):
-        self.uid = uid
-        self.link_id = link_id
-
-        f = width / (2.0 * np.tan(hfov / 2.0))
-        cx, cy = width / 2.0, height / 2.0
-
-        self.width = width
-        self.height = height
-        self.K = [f, 0.0, cx, 0.0, f, cy, 0.0, 0.0, 1.0]
-        self.near = near
-        self.far = far
-
-        self.proj_mat = p.computeProjectionMatrixFOV(
-            np.rad2deg(hfov), width / height, near, far
-        )
-
-    def update(self):
-        result = p.getLinkState(self.uid, self.link_id, computeForwardKinematics=1)
-        t_world_cam = result[4]
-        R_world_cam = Rotation.from_quat(result[5])
-        eye = t_world_cam
-        target = R_world_cam.apply(np.r_[0.0, 0.0, 1.0]) + t_world_cam
-        up = R_world_cam.apply(np.r_[0.0, -1.0, 0.0])
-        view_mat = p.computeViewMatrix(eye, target, up)
-
-        result = p.getCameraImage(
-            self.width,
-            self.height,
-            view_mat,
-            self.proj_mat,
-            renderer=p.ER_BULLET_HARDWARE_OPENGL,
-        )
-        color_img, z_buffer = result[2][:, :, :3], result[3]
-
-        color_img = color_img.astype(np.uint8)
-        depth_img = (
-            self.far * self.near / (self.far - (self.far - self.near) * z_buffer)
-        ).astype(np.float32)
-
-        return color_img, depth_img
-
-
-class SimPandaEnv(object):
-    def __init__(self, gui, publish_state=True):
-        self.gui = gui
-        self.rate = 240
-        p.connect(p.GUI if gui else p.DIRECT)
-        p.setAdditionalSearchPath(pybullet_data.getDataPath())
-        p.setTimeStep(1.0 / self.rate)
-        p.setGravity(0.0, 0.0, -9.8)
-        self.step_cnt = 0
-
-        p.loadURDF("plane.urdf")
-        p.loadURDF(
-            "table/table.urdf",
-            baseOrientation=Rotation.from_rotvec(np.array([0, 0, np.pi / 2])).as_quat(),
-            useFixedBase=True,
-        )
-        p.loadURDF("cube_small.urdf", [0.0, 0.0, 0.8])
-
-        self.arm = PandaArm()
-        self.arm.load(Transform(Rotation.identity(), np.r_[-0.8, 0.0, 0.4]))
-        self.gripper = PandaGripper()
-        self.gripper.uid = self.arm.uid
-        self.model = Model("panda_link0", "panda_ee")
-        self.camera = Camera(self.arm.uid, 12, 320, 240, 1.047, 0.2, 2.0)
-
-        if publish_state:
-            self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=10)
-            rospy.Timer(rospy.Duration(1.0 / 30), self._publish_state)
-
-    def forward(self, dt):
-        steps = int(dt * self.rate)
-        for _ in range(steps):
-            self._step()
-
-    def _step(self):
-        if self.step_cnt % int(self.rate / self.controller.rate) == 0:
-            self.controller.update()
-        p.stepSimulation()
-        time.sleep(1.0 / self.rate)
-
-    def _publish_state(self, event):
-        positions, velocities = self.arm.get_state()
-        width = self.gripper.read()
-        msg = JointState()
-        msg.header.stamp = rospy.Time.now()
-        msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [
-            "panda_finger_joint1",
-            "panda_finger_joint2",
-        ]
-        msg.position = np.r_[positions, 0.5 * width, 0.5 * width]
-        msg.velocity = velocities
-        self.state_pub.publish(msg)

run.py

@@ -0,0 +1,31 @@
+import argparse
+
+from geometry_msgs.msg import Pose
+import numpy as np
+import rospy
+from std_srvs.srv import Trigger
+
+from policies import get_policy
+
+
+def main(args):
+    rospy.init_node("panda_grasp")
+
+    policy = get_policy(args.policy)
+
+    r = rospy.Rate(args.rate)
+    done = False
+    policy.start()
+    while not done:
+        done = policy.update()
+        r.sleep()
+
+    # TODO execute grasp
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--policy", type=str, choices=["fixed"])
+    parser.add_argument("--rate", type=int, default=10)
+    args = parser.parse_args()
+    main(args)

run_demo.py

@@ -1,23 +0,0 @@
-import rospy
-
-from controllers import *
-from robot_sim import *
-from utils import *
-
-
-gui = True
-
-rospy.init_node("demo")
-
-env = SimPandaEnv(gui)
-
-q, dq = env.arm.get_state()
-x0 = env.model.pose(q)
-
-env.controller = CartesianPoseController(env.arm, env.model, x0, 60)
-marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
-
-while True:
-    env.controller.set_target(marker.pose)
-    env.camera.update()
-    env.forward(0.1)

sim.py

@@ -0,0 +1,69 @@
+import argparse
+
+import numpy as np
+import rospy
+
+from geometry_msgs.msg import Pose
+from sensor_msgs.msg import JointState
+
+from robot_tools.btsim import BtPandaEnv
+from robot_tools.controllers import CartesianPoseController
+from robot_tools.ros import *
+
+CONTROLLER_UPDATE_RATE = 60
+JOINT_STATE_PUBLISHER_RATE = 60
+
+
+class BtSimNode:
+    def __init__(self, gui):
+        self.sim = BtPandaEnv(gui=gui, sleep=False)
+        self.controller = CartesianPoseController(self.sim.arm)
+
+        self.joint_state_pub = rospy.Publisher(
+            "/joint_states", JointState, queue_size=10
+        )
+        rospy.Subscriber("/target", Pose, self._target_pose_cb)
+
+    def run(self):
+        rate = rospy.Rate(self.sim.rate)
+        self.step_cnt = 0
+        while not rospy.is_shutdown():
+            self._handle_updates()
+            self.sim.step()
+            self.step_cnt = (self.step_cnt + 1) % self.sim.rate
+            rate.sleep()
+
+    def _handle_updates(self):
+        if self.step_cnt % int(self.sim.rate / CONTROLLER_UPDATE_RATE) == 0:
+            self.controller.update()
+        if self.step_cnt % int(self.sim.rate / JOINT_STATE_PUBLISHER_RATE) == 0:
+            self._publish_joint_state()
+
+    def _publish_joint_state(self):
+        q, dq = self.sim.arm.get_state()
+        width = self.sim.gripper.read()
+        msg = JointState()
+        msg.header.stamp = rospy.Time.now()
+        msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [
+            "panda_finger_joint1",
+            "panda_finger_joint2",
+        ]
+        msg.position = np.r_[q, 0.5 * width, 0.5 * width]
+        msg.velocity = dq
+        self.joint_state_pub.publish(msg)
+
+    def _target_pose_cb(self, msg):
+        self.controller.set_target(from_pose_msg(msg))
+
+
+def main(args):
+    rospy.init_node("bt_sim")
+    server = BtSimNode(args.gui)
+    server.run()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--gui", type=str, default=True)
+    args = parser.parse_args()
+    main(args)

utils.py

@@ -1,214 +0,0 @@
-import numpy as np
-from scipy.spatial.transform import Rotation
-import PyKDL as kdl
-
-import geometry_msgs.msg
-from interactive_markers.interactive_marker_server import *
-from interactive_markers.menu_handler import *
-import std_msgs.msg
-from visualization_msgs.msg import *
-
-
-class InteractiveMarkerWrapper(object):
-    def __init__(self, name, frame_id, x0):
-        self.pose = x0
-
-        server = InteractiveMarkerServer(topic_ns=name)
-
-        int_marker = InteractiveMarker()
-        int_marker.header.frame_id = frame_id
-        int_marker.name = name
-        int_marker.scale = 0.2
-        int_marker.pose = to_pose_msg(x0)
-
-        # Attach visible sphere
-        marker = Marker()
-        marker.type = Marker.SPHERE
-        marker.scale = to_vector3_msg([0.05, 0.05, 0.05])
-        marker.color = to_color_msg([0.0, 0.5, 0.5, 0.6])
-
-        ctrl = InteractiveMarkerControl()
-        ctrl.always_visible = True
-        ctrl.markers.append(marker)
-        int_marker.controls.append(ctrl)
-
-        # Attach rotation controls
-        ctrl = InteractiveMarkerControl()
-        ctrl.name = "rotate_x"
-        ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1]))
-        ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
-        int_marker.controls.append(ctrl)
-
-        ctrl = InteractiveMarkerControl()
-        ctrl.name = "rotate_y"
-        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1]))
-        ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
-        int_marker.controls.append(ctrl)
-
-        ctrl = InteractiveMarkerControl()
-        ctrl.name = "rotate_z"
-        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1]))
-        ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
-        int_marker.controls.append(ctrl)
-
-        # Attach translation controls
-        ctrl = InteractiveMarkerControl()
-        ctrl.name = "move_x"
-        ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1]))
-        ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-        int_marker.controls.append(ctrl)
-
-        ctrl = InteractiveMarkerControl()
-        ctrl.name = "move_y"
-        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1]))
-        ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-        int_marker.controls.append(ctrl)
-
-        ctrl = InteractiveMarkerControl()
-        ctrl.name = "move_z"
-        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1]))
-        ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-        int_marker.controls.append(ctrl)
-
-        server.insert(int_marker, self.cb)
-        server.applyChanges()
-
-    def cb(self, feedback):
-        self.pose = from_pose_msg(feedback.pose)
-
-
-class Transform(object):
-    def __init__(self, rotation, translation):
-        self.rotation = rotation
-        self.translation = np.asarray(translation, np.double)
-
-    def as_matrix(self):
-        return np.vstack(
-            (np.c_[self.rotation.as_matrix(), self.translation], [0.0, 0.0, 0.0, 1.0])
-        )
-
-    def to_list(self):
-        return np.r_[self.rotation.as_quat(), self.translation]
-
-    def __mul__(self, other):
-        rotation = self.rotation * other.rotation
-        translation = self.rotation.apply(other.translation) + self.translation
-        return self.__class__(rotation, translation)
-
-    def inverse(self):
-        rotation = self.rotation.inv()
-        translation = -rotation.apply(self.translation)
-        return self.__class__(rotation, translation)
-
-    @classmethod
-    def identity(cls):
-        rotation = Rotation.from_quat([0.0, 0.0, 0.0, 1.0])
-        translation = np.array([0.0, 0.0, 0.0])
-        return cls(rotation, translation)
-
-    @classmethod
-    def from_matrix(cls, m):
-        rotation = Rotation.from_matrix(m[:3, :3])
-        translation = m[:3, 3]
-        return cls(rotation, translation)
-
-    @classmethod
-    def from_list(cls, list):
-        rotation = Rotation.from_quat(list[:4])
-        translation = list[4:]
-        return cls(rotation, translation)
-
-    @classmethod
-    def from_kdl(cls, f):
-        rotation = Rotation.from_matrix(
-            np.array(
-                [
-                    [f.M[0, 0], f.M[0, 1], f.M[0, 2]],
-                    [f.M[1, 0], f.M[1, 1], f.M[1, 2]],
-                    [f.M[2, 0], f.M[2, 1], f.M[2, 2]],
-                ]
-            )
-        )
-        translation = np.r_[f.p[0], f.p[1], f.p[2]]
-        return cls(rotation, translation)
-
-
-# KDL Conversions
-
-
-def to_kdl_jnt_array(q):
-    jnt_array = kdl.JntArray(len(q))
-    for i, q_i in enumerate(q):
-        jnt_array[i] = q_i
-    return jnt_array
-
-
-def kdl_to_mat(m):
-    mat = np.zeros((m.rows(), m.columns()))
-    for i in range(m.rows()):
-        for j in range(m.columns()):
-            mat[i, j] = m[i, j]
-    return mat
-
-
-# ROS Conversions
-
-
-def to_color_msg(color):
-    msg = std_msgs.msg.ColorRGBA()
-    msg.r = color[0]
-    msg.g = color[1]
-    msg.b = color[2]
-    msg.a = color[3]
-    return msg
-
-
-def to_point_msg(point):
-    msg = geometry_msgs.msg.Point()
-    msg.x = point[0]
-    msg.y = point[1]
-    msg.z = point[2]
-    return msg
-
-
-def from_point_msg(msg):
-    return np.r_[msg.x, msg.y, msg.z]
-
-
-def to_vector3_msg(vector3):
-    msg = geometry_msgs.msg.Vector3()
-    msg.x = vector3[0]
-    msg.y = vector3[1]
-    msg.z = vector3[2]
-    return msg
-
-
-def from_vector3_msg(msg):
-    return np.r_[msg.x, msg.y, msg.z]
-
-
-def to_quat_msg(orientation):
-    quat = orientation.as_quat()
-    msg = geometry_msgs.msg.Quaternion()
-    msg.x = quat[0]
-    msg.y = quat[1]
-    msg.z = quat[2]
-    msg.w = quat[3]
-    return msg
-
-
-def from_quat_msg(msg):
-    return Rotation.from_quat([msg.x, msg.y, msg.z, msg.w])
-
-
-def to_pose_msg(transform):
-    msg = geometry_msgs.msg.Pose()
-    msg.position = to_point_msg(transform.translation)
-    msg.orientation = to_quat_msg(transform.rotation)
-    return msg
-
-
-def from_pose_msg(msg):
-    position = from_point_msg(msg.position)
-    orientation = from_quat_msg(msg.orientation)
-    return Transform(orientation, position)