cm

python/examples/clik.py

@@ -5,7 +5,7 @@ import pinocchio as pin
 import argcomplete, argparse
 from functools import partial
 from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, moveLDualArm
 from ur_simple_control.util.define_random_goal import getRandomlyGeneratedGoal
 
 

python/ur_simple_control/managers.py

@@ -281,8 +281,13 @@ class ControlLoopManager:
         if i % 20 == 0:
             # don't send what wasn't ready
             if self.args.visualize_manipulator:
+                if self.robot_name != "yumi":
                     self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
                                                           "T_w_e" : self.robot_manager.getT_w_e()})
+                else self.robot_name != "yumi":
+                    T_w_e_left, T_w_e_right = self.robot_manager.getT_w_e()
+                    self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
+                                                          "T_w_e" : T_w_e_left})
                 if self.robot_manager.robot_name == "heron":
                     T_base = self.robot_manager.data.oMi[1]
                     self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})