jj

python/examples/smc_yumi_challenge.py

@@ -13,6 +13,7 @@ from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager,
 from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP, moveLDualArm
 from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
 from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
+import pinocchio as pin
 
 
 def get_args():
@@ -33,9 +34,9 @@ class ROSCommHandlerMYumi(Node):
         self._ns = get_rosified_name(self.get_namespace())
         self.get_logger().info(f"### Starting dummy example under namespace {self._ns}")
         
-        self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}/platform/base_command", 1)
-        self.pub_vel_left = self.create_publisher(JointState, f"{self._ns}/platform/left_arm_command", 1)
-        self.pub_vel_right = self.create_publisher(JointState, f"{self._ns}/platform/right_arm_command", 1)
+        self.pub_vel_base = self.create_publisher(Twist, f"{self._ns}platform/base_command", 1)
+        self.pub_vel_left = self.create_publisher(JointState, f"{self._ns}platform/left_arm_command", 1)
+        self.pub_vel_right = self.create_publisher(JointState, f"{self._ns}platform/right_arm_command", 1)
         robot_manager.set_publisher_vel_base(self.pub_vel_base)
         robot_manager.set_publiser_vel_left(self.pub_vel_left)
         robot_manager.set_publiser_vel_right(self.pub_vel_right)
@@ -53,9 +54,9 @@ class ROSCommHandlerMYumi(Node):
 
         # MASSIVE TODO: you want to be subsribed to a proper localization thing,
         # specifically to output from robot_localization
-        self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}/platform/odometry", self.callback_base_odom, qos_prof)
+        self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}platform/odometry", self.callback_base_odom, qos_prof)
         # self.sub_amcl = self.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.pose_callback, qos_prof)
-        self.sub_arms_state = self.create_subscription(JointState, f"{self._ns}/platform/joint_states", self.callback_arms_state, qos_prof)
+        self.sub_arms_state = self.create_subscription(JointState, f"{self._ns}platform/joint_states", self.callback_arms_state, qos_prof)
         self.names_left = {f"{self._ns}_yumi_robl_joint_{i+1}": i for i in range(7)}
         self.state_left = JointState(name=list(sorted(self.names_left.keys())),
                                      position=[0] * 7,