added go to initial pose in clik_client.py

python/examples/graz/clik_client.py

@@ -20,6 +20,11 @@ def get_args():
     parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1")
     parser.add_argument("--port-wrench", type=int, help="host's port", default=6666)
     parser.add_argument("--port-command", type=int, help="host's port", default=7777)
+    # put your own default here
+    # it's translation + orientation as a quarternion
+    parser.add_argument(
+        "--init-pose", type=float, nargs=7, default=[0.3, 0.3, 0.3, 0.0, 0.0, 0.0, 1.0]
+    )
     args = parser.parse_args()
     return args
 
@@ -82,10 +87,11 @@ def controlLoopClikExternalGoal(
 if __name__ == "__main__":
     args = get_args()
     robot = RobotManager(args)
-
-    # get expected behaviour here (library can't know what the end is - you have to do this here)
-    if not args.pinocchio_only:
-        robot.stopRobot()
+    translation = np.array(args.init_pose[:3])
+    orientation = pin.Quaternion(np.array(args.init_pose[3:]))
+    init_goal = pin.SE3(orientation, translation)
+    moveL(args, robot, init_goal)
+    print("arrived at initial position")
 
     # VERY important that the first q we'll pass as desired is the current q, meaning the robot won't move
     # this is set with init_value
@@ -113,6 +119,10 @@ if __name__ == "__main__":
     loop_manager = ControlLoopManager(robot, control_loop, args, {}, log_item)
     loop_manager.run()
 
+    # get expected behaviour here (library can't know what the end is - you have to do this here)
+    if not args.pinocchio_only:
+        robot.stopRobot()
+
     if args.save_log:
         robot.log_manager.plotAllControlLoops()