Js placo (#880)

* moved localization and firmware into folders where they belong. playing with order of folders. added an ros example that makes sense for camera. setting up a ros example that makes sense with walk but the ros layer needs some work

* experimenting with placo

* working on adding placo as the main walk engine

* Fixed someproblems in the webots bridge and urdf/xacro. integrated placo as a walk engine alternative

* added a rough draft of ros layer for placo and it works great out of the box

* removed old walk engine and replaced with placo. reworked the ros layer and removed redundent parameter sharing. General clean up. Got walking loop to work from same code regardless of sim or ros. Fixed parameters and general plumbing. removed sigmanban and other description stuff to make it nicer. Fixed bez2 xacro and some improvements to firmware interface tests. should be ready to go.

* renamed motor names in trajecotory and made a arm waving traj. started to investigate the inverse perspective mapping for transforming the ball and others from camerea image to robot frame. ball works but when looking at the generic version it realies on an accurate z height of robot since this is used for the projecton to the floor. Some refactor to firmware interface and added a lock. general cleanup

soccer_control/soccer_pycontrol/config/bez2/bez2.yaml

@@ -33,7 +33,7 @@ planned_timesteps: 48 # Number of timesteps planned ahead
 replan_timesteps: 10 # Replanning each n timesteps
 
 # Posture parameters
-walk_com_height: 0.25 # Constant height for the CoM [m]
+walk_com_height: 0.27 # Constant height for the CoM [m]
 walk_foot_height: 0.04 # Height of foot rising while walking [m]
 walk_trunk_pitch: 0.0 # Trunk pitch angle [rad]
 walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)

soccer_control/soccer_pycontrol/config/bez2/bez2_sim.yaml

@@ -33,7 +33,7 @@ planned_timesteps: 48 # Number of timesteps planned ahead
 replan_timesteps: 10 # Replanning each n timesteps
 
 # Posture parameters
-walk_com_height: 0.25 # Constant height for the CoM [m]
+walk_com_height: 0.27 # Constant height for the CoM [m]
 walk_foot_height: 0.04 # Height of foot rising while walking [m]
 walk_trunk_pitch: 0.0 # Trunk pitch angle [rad]
 walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)

soccer_control/soccer_pycontrol/config/bez2/bez2_sim_pybullet.yaml

@@ -33,7 +33,7 @@ planned_timesteps: 48 # Number of timesteps planned ahead
 replan_timesteps: 10 # Replanning each n timesteps
 
 # Posture parameters
-walk_com_height: 0.25 # Constant height for the CoM [m]
+walk_com_height: 0.27 # Constant height for the CoM [m]
 walk_foot_height: 0.04 # Height of foot rising while walking [m]
 walk_trunk_pitch: 0.0 # Trunk pitch angle [rad]
 walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)
@@ -42,7 +42,7 @@ walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)
 foot_length: 0.1576 # Foot length [m]
 foot_width: 0.092 # Foot width [m]
 feet_spacing: 0.122 # Lateral feet spacing [m]
-zmp_margin: 0.02 # ZMP margin [m]
+zmp_margin: 0.01 # ZMP margin [m]
 foot_zmp_target_x: 0.0 # Reference target ZMP position in the foot [m]
 foot_zmp_target_y: 0.0 # Reference target ZMP position in the foot [m]
 

soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/placo_joint_tasks.py

@@ -6,7 +6,7 @@
 from ischedule import run_loop, schedule
 from placo_utils.visualization import point_viz, robot_frame_viz, robot_viz
 
-robot_model = "bez1"
+robot_model = "bez2"
 model_filename = expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf"
 
 robot = placo.HumanoidRobot(model_filename)
@@ -20,16 +20,17 @@
 # Retrieving initial position of the feet, com and trunk orientation
 # T_world_left = robot.get_T_world_frame("left_foot")
 # T_world_right = robot.get_T_world_frame("right_foot")
-robot.get_T_world_trunk()
 # Creating the viewer
+T_world_trunk = robot.get_T_world_frame("trunk")
 viz = robot_viz(robot)
-T_world_trunk = robot.get_T_world_trunk()
 # Trunk
-com_task = solver.add_com_task(T_world_trunk[:3, 3])
-com_task.configure("com", "hard", 1.0)
+# com_task = solver.add_com_task(T_world_trunk[:3, 3])
+# com_task.configure("com", "hard", 1.0)
 
-trunk_orientation_task = solver.add_orientation_task("trunk", np.eye(3))
-trunk_orientation_task.configure("trunk_orientation", "soft", 1.0)
+
+# T_world_trunk[2, 3] = 0.35
+trunk_task = solver.add_frame_task("trunk", T_world_trunk)
+trunk_task.configure("trunk_task", "hard", 1e3, 1e3)
 
 # Keep left and right foot on the floor
 left_foot_task = solver.add_frame_task("left_foot", T_world_left)
@@ -38,27 +39,48 @@
 right_foot_task = solver.add_frame_task("right_foot", T_world_right)
 right_foot_task.configure("right_foot", "soft", 1.0, 1.0)
 
-head_task = solver.add_joints_task()
-elbow = 2.512
-shoulder_pitch = -0.45
-head_task.set_joints(
-    {
-        "left_shoulder_pitch": shoulder_pitch,
-        "left_elbow": elbow,
-        # "right_shoulder_roll": -shoulder_roll,
-        "right_shoulder_pitch": shoulder_pitch,
-        "right_elbow": elbow,
-        "head_pitch": 0.0,
-        "head_yaw": 0.0,
-    }
-)
-head_task.configure("joints", "soft", 1.0)
-
+T_world_right_forearm = robot.get_T_world_frame("right_forearm")
+right_hand_task = solver.add_frame_task("right_forearm", T_world_right_forearm)
+right_hand_task.configure("right_forearm", "soft", 1.0, 1.0)
+# right_forearm
+# head_task = solver.add_joints_task()
+# elbow = 2.512
+# shoulder_pitch = -0.45
+# head_task.set_joints(
+#     {
+#         # "left_shoulder_pitch": shoulder_pitch,
+#         # "left_elbow": elbow,
+#         # "right_shoulder_roll": -shoulder_roll,
+#         # "right_shoulder_pitch": shoulder_pitch,
+#         # "right_elbow": elbow,
+#         "head_pitch": 0.0,
+#         "head_yaw": 0.0,
+#     }
+# )
+# head_task.configure("joints", "soft", 1.0)
+x, y, z = T_world_right_forearm[:3, 3].copy()
+right_hand_y_traj = placo.CubicSpline()
+right_hand_y_traj.add_point(0.0, y, 0.0)
+right_hand_y_traj.add_point(0.5, y - 0.5, 0.0)
+right_hand_y_traj.add_point(1.0, y, 0.0)
+right_hand_y_traj.add_point(2.0, y, 0.0)
+
+right_hand_x_traj = placo.CubicSpline()
+right_hand_x_traj.add_point(0.0, x + 5, 0.0)
+right_hand_x_traj.add_point(0.5, x + 5, 0.0)
+right_hand_x_traj.add_point(1.0, x + 5, 0.0)
+right_hand_x_traj.add_point(2.0, x + 5, 0.0)
+
+right_hand_z_traj = placo.CubicSpline()
+right_hand_z_traj.add_point(0.0, z + 0.3, 0.0)
+right_hand_z_traj.add_point(0.5, z + 0.3, 0.0)
+right_hand_z_traj.add_point(1.0, z + 0.3, 0.0)
+right_hand_z_traj.add_point(2.0, z + 0.1, 0.0)
 
 # Regularization task
-posture_regularization_task = solver.add_joints_task()
-posture_regularization_task.set_joints({dof: 0.0 for dof in robot.joint_names()})
-posture_regularization_task.configure("reg", "soft", 1e-5)
+# posture_regularization_task = solver.add_joints_task()
+# posture_regularization_task.set_joints({dof: 0.0 for dof in robot.joint_names()})
+# posture_regularization_task.configure("reg", "soft", 1e-5)
 
 solver.enable_joint_limits(True)
 solver.enable_velocity_limits(True)
@@ -76,10 +98,17 @@ def loop():
     global t
 
     # Updating the com target with lateral sinusoidal trajectory
-    com_target = T_world_trunk[:3, 3].copy()
-    com_target[1] = 0.21
-    com_target[2] = 0.21  # T_world_trunk[2,3] - 0.1
-    com_task.target_world = com_target
+    # com_target = T_world_trunk[:3, 3].copy()
+    # com_target[1] = 0.21
+    # com_target[2] = 0.21  # T_world_trunk[2,3] - 0.1
+    # com_task.target_world = com_target
+    # Updating the target
+    t_mod = t % 2.0
+    target = right_hand_task.position().target_world
+    # target[0] = right_hand_x_traj.pos(t_mod)
+    # target[1] = right_hand_y_traj.pos(t_mod)
+    # target[2] = right_hand_z_traj.pos(t_mod)
+    right_hand_task.position().target_world = target
 
     solver.solve(True)
     robot.update_kinematics()

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py

@@ -13,7 +13,7 @@ def __init__(self, robot_model: str, parameters: dict, funct_time, debug: bool =
         self.DT = parameters["control_frequency"]
 
         self.debug = debug
-
+        self.robot_model = robot_model
         model_filename = expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf"
         self.parameters = self.walk_parameters(parameters)
 
@@ -82,18 +82,32 @@ def setup_tasks(self):
         shoulder_roll = 0 * np.pi / 180
         shoulder_pitch = 20 * np.pi / 180
         joints_task = self.solver.add_joints_task()
-        joints_task.set_joints(
-            {
-                # "left_shoulder_roll": shoulder_roll,
-                "left_shoulder_pitch": shoulder_pitch,
-                "left_elbow": elbow,
-                # "right_shoulder_roll": -shoulder_roll,
-                "right_shoulder_pitch": shoulder_pitch,
-                "right_elbow": elbow,
-                "head_pitch": 0.0,
-                "head_yaw": 0.0,
-            }
-        )
+        if self.robot_model == "bez2":
+            joints_task.set_joints(
+                {
+                    "left_shoulder_roll": shoulder_roll,
+                    "left_shoulder_pitch": shoulder_pitch,
+                    "left_elbow": elbow,
+                    "right_shoulder_roll": -shoulder_roll,
+                    "right_shoulder_pitch": shoulder_pitch,
+                    "right_elbow": elbow,
+                    "head_pitch": 0.0,
+                    "head_yaw": 0.0,
+                }
+            )
+        else:
+            joints_task.set_joints(
+                {
+                    # "left_shoulder_roll": shoulder_roll,
+                    "left_shoulder_pitch": shoulder_pitch,
+                    "left_elbow": elbow,
+                    # "right_shoulder_roll": -shoulder_roll,
+                    "right_shoulder_pitch": shoulder_pitch,
+                    "right_elbow": elbow,
+                    "head_pitch": 0.0,
+                    "head_yaw": 0.0,
+                }
+            )
         joints_task.configure("joints", "soft", 1.0)
 
         # Placing the robot in the initial position

soccer_control/soccer_pycontrol/test/test_walk_ros.py

@@ -26,7 +26,7 @@ def test_walk_ros_local(self):
         # bez.motor_control.set_motor()
         # walker.wait(50)
         # walker.goal_callback(PoseStamped())
-        walker.walk(d_x=0.05, t_goal=10)
+        walker.walk(d_x=0.03, t_goal=10)
         # walker.walk(d_x=0.0, d_y=0.06, t_goal=5)
         # walker.walk(d_x=0.0, d_theta=0.3, t_goal=5)
         # walker.walk(d_x=-0.04, t_goal=5)

soccer_control/soccer_trajectories/test/test_trajectory.py

@@ -26,17 +26,17 @@ def test_trajectory(self):
         #
         joint_state = JointState()
         joint_state.name = [
-            "right_leg_motor_0",
+            "right_hip_yaw",
             "left_leg_motor_0",
-            "right_leg_motor_1",
+            "right_hip_roll",
             "left_leg_motor_1",
-            "right_leg_motor_2",
+            "right_hip_pitch",
             "left_leg_motor_2",
-            "right_leg_motor_3",
+            "right_knee",
             "left_leg_motor_3",
-            "right_leg_motor_4",
+            "right_ankle_pitch",
             "left_leg_motor_4",
-            "right_leg_motor_5",
+            "right_ankle_roll",
             "left_leg_motor_5",
             "right_arm_motor_0",
             "left_arm_motor_0",
@@ -52,7 +52,7 @@ def test_trajectory(self):
         self.assertEqual(angles, [0.0, 0.0, 0.0, 0.0, 0.564, 0.564, -1.176, -1.176, 0.613, 0.613, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
 
 
-@pytest.mark.parametrize("trajectory_name", ["getupfront"])  # , "getupback_full", "rightkick", "getupfront"])
+@pytest.mark.parametrize("trajectory_name", ["fix_angle_test"])  # , "getupback_full", "rightkick", "getupfront"])
 @pytest.mark.parametrize("robot_model", ["bez2"])
 @pytest.mark.parametrize("real_time", [True])
 def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool):
@@ -67,9 +67,9 @@ def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool)
     elif trajectory_name == "getupback_full" or trajectory_name == "getupback_roll":
         pose = Transformation(position=[0, 0, 0.070], euler=[0, -1.57, 0])
         camera = 0
-    # else:
-    #     pose = Transformation(position=[0, 0, 0.315], quaternion=[0.0, 0.0, 0.0, 1])
-
+    else:
+        camera = 90
+        pose = Transformation(position=[0, 0, 0.45], quaternion=[0.0, 0.0, 0.0, 1])
     print(os.path.join(os.path.dirname(__file__), "../trajectories/bez2/" + trajectory_name + ".csv"))
     tm = TrajectoryManagerSim(
         os.path.join(os.path.dirname(__file__), "../trajectories/bez2/" + trajectory_name + ".csv"), pose, robot_model, real_time, camera_yaw=camera

soccer_control/soccer_trajectories/trajectories/bez1/getupback.csv

@@ -1,15 +1,15 @@
 time,0,1,2,3,4,5,6,6.75,7.5,8.25,9,9.75,10.5,11.25,12,12.75,13.5,14.25,15,15.75,16.5,17.25,18,18.75,19.5
-right_leg_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 left_leg_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-right_leg_motor_1,0.2,0.2,0.2,-0.4,-0.4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.05,-0.05
+right_hip_roll,0.2,0.2,0.2,-0.4,-0.4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.05,-0.05
 left_leg_motor_1,0,0,0,-0.1,0.7,0.7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.05,-0.05
-right_leg_motor_2,0,1.5,1.8,1.8,1.8,0,0,0,0,0,0,1,1.5,1.5,1.5,1.8,1.8,1.8,1.8,1.8,1.5,1.2,0.9,0.75,0.6
+right_hip_pitch,0,1.5,1.8,1.8,1.8,0,0,0,0,0,0,1,1.5,1.5,1.5,1.8,1.8,1.8,1.8,1.8,1.5,1.2,0.9,0.75,0.6
 left_leg_motor_2,0,1.5,-0.5,-0.5,1,1,0,0,0,0,0,1,1.5,1.5,1.5,1.8,1.8,1.8,1.8,1.8,1.5,1.2,0.9,0.75,0.6
-right_leg_motor_3,0,-1.8,-1.8,-1.8,-1.8,-1.8,0,0,0,0,0,-1,-1.3,-1.3,-1.3,-1.8,-1.8,-1.8,-1.8,-1.8,-1.8,-1.5,-1.2,-1.3,-1.4
+right_knee,0,-1.8,-1.8,-1.8,-1.8,-1.8,0,0,0,0,0,-1,-1.3,-1.3,-1.3,-1.8,-1.8,-1.8,-1.8,-1.8,-1.8,-1.5,-1.2,-1.3,-1.4
 left_leg_motor_3,0,-1.2,-1.2,-0.4,-0.4,-0.4,0,0,0,0,0,-1,-1.3,-1.3,-1.3,-1.8,-1.8,-1.8,-1.8,-1.8,-1.8,-1.5,-1.2,-1.3,-1.4
-right_leg_motor_4,0,0.9,0.9,0.9,0.9,0.9,0,0,0,0,1.1,1.3,1.3,1.3,1.3,1.3,1.3,1.3,0.9,0.6,0.6,0.5,0.46,0.6,0.8
+right_ankle_pitch,0,0.9,0.9,0.9,0.9,0.9,0,0,0,0,1.1,1.3,1.3,1.3,1.3,1.3,1.3,1.3,0.9,0.6,0.6,0.5,0.46,0.6,0.8
 left_leg_motor_4,0,0.9,0.9,1.3,1.3,1.3,0,0,0,0,1.1,1.3,1.3,1.3,1.3,1.3,1.3,1.3,0.9,0.6,0.6,0.5,0.46,0.6,0.8
-right_leg_motor_5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05,0
+right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05,0
 left_leg_motor_5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05,0
 right_arm_motor_0,0,1,-2,0,2,3,3,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.5,0,0.3,0.8,0.7,0.6,0.5,0.6,0.75
 left_arm_motor_0,0,1,1,3,3,3,3,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.5,0,0.3,0.8,0.7,0.6,0.5,0.6,0.75

soccer_control/soccer_trajectories/trajectories/bez1/getupfront.csv

@@ -1,15 +1,15 @@
 time,0,0.75,1.5,2.25,3,3.75,4.5,5.25,6,6.75,7.5,8.25,9,9.75,10.5,11.25,12,12.75,13.5
-right_leg_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 left_leg_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-right_leg_motor_1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.05,-0.05
+right_hip_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.05,-0.05
 left_leg_motor_1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.05,-0.05
-right_leg_motor_2,0,0,0,0,0,1,1.5,1.5,1.5,1.8,1.8,1.8,1.8,1.8,1.5,1.2,0.9,0.75,0.6
+right_hip_pitch,0,0,0,0,0,1,1.5,1.5,1.5,1.8,1.8,1.8,1.8,1.8,1.5,1.2,0.9,0.75,0.6
 left_leg_motor_2,0,0,0,0,0,1,1.5,1.5,1.5,1.8,1.8,1.8,1.8,1.8,1.5,1.2,0.9,0.75,0.6
-right_leg_motor_3,0,0,0,0,0,-1,-1.3,-1.3,-1.3,-1.8,-1.8,-1.8,-1.8,-1.8,-1.8,-1.5,-1.2,-1.3,-1.4
+right_knee,0,0,0,0,0,-1,-1.3,-1.3,-1.3,-1.8,-1.8,-1.8,-1.8,-1.8,-1.8,-1.5,-1.2,-1.3,-1.4
 left_leg_motor_3,0,0,0,0,0,-1,-1.3,-1.3,-1.3,-1.8,-1.8,-1.8,-1.8,-1.8,-1.8,-1.5,-1.2,-1.3,-1.4
-right_leg_motor_4,0,0,0,0,1.1,1.3,1.3,1.3,1.3,1.3,1.3,1.3,0.9,0.6,0.6,0.5,0.46,0.6,0.8
+right_ankle_pitch,0,0,0,0,1.1,1.3,1.3,1.3,1.3,1.3,1.3,1.3,0.9,0.6,0.6,0.5,0.46,0.6,0.8
 left_leg_motor_4,0,0,0,0,1.1,1.3,1.3,1.3,1.3,1.3,1.3,1.3,0.9,0.6,0.6,0.5,0.46,0.6,0.8
-right_leg_motor_5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05,0
+right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05,0
 left_leg_motor_5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05,0
 right_arm_motor_0,0,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.5,0,0.3,0.8,0.7,0.6,0.5,0.6,0.75
 left_arm_motor_0,0,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.5,0,0.3,0.8,0.7,0.6,0.5,0.6,0.75

soccer_control/soccer_trajectories/trajectories/bez1/getupside.csv

@@ -1,15 +1,15 @@
 time,0,0.5
-right_leg_motor_0,0,0
+right_hip_yaw,0,0
 left_leg_motor_0,0,0
-right_leg_motor_1,0,0
+right_hip_roll,0,0
 left_leg_motor_1,0,0
-right_leg_motor_2,0,0
+right_hip_pitch,0,0
 left_leg_motor_2,0,0
-right_leg_motor_3,0,0
+right_knee,0,0
 left_leg_motor_3,0,0
-right_leg_motor_4,0,0
+right_ankle_pitch,0,0
 left_leg_motor_4,0,0
-right_leg_motor_5,0,0
+right_ankle_roll,0,0
 left_leg_motor_5,0,0
 right_arm_motor_0,0,-1.5708
 left_arm_motor_0,0,-1.5708

soccer_control/soccer_trajectories/trajectories/bez1/rightkick.csv

@@ -7,12 +7,12 @@ left_leg_motor_4,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2
 left_leg_motor_5,-0.1,-0.1,0.15,0.15,0.25,0.25,0.25,0.25
 left_arm_motor_0,0,0,0,0,-1,-1,1,1
 left_arm_motor_1,2,2,2,2,2,2,2,2
-right_leg_motor_0,0,0,0,0,0,0,0,0
-right_leg_motor_1,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4
-right_leg_motor_2,0.25,0.25,0.25,0.25,0.3,0.3,1.5,1
-right_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-1,-1,-1,-1
-right_leg_motor_4,0.2,0.2,0.2,0.2,0.5,0.5,0,0
-right_leg_motor_5,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2
+right_hip_yaw,0,0,0,0,0,0,0,0
+right_hip_roll,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4
+right_hip_pitch,0.25,0.25,0.25,0.25,0.3,0.3,1.5,1
+right_knee,-0.4,-0.4,-0.4,-0.4,-1,-1,-1,-1
+right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,0.5,0,0
+right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2
 right_arm_motor_0,0,0,0,0,1,1,-1,-1
 right_arm_motor_1,2,2,2,2,2,2,2,2
 comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance

soccer_control/soccer_trajectories/trajectories/bez1/rightkick_2.csv

@@ -7,12 +7,12 @@ left_leg_motor_4,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2
 left_leg_motor_5,-0.1,-0.1,0.15,0.15,0.25,0.25,0.25,0.25,-0.1
 left_arm_motor_0,0,0,0,0,-1,-1,1,1,0
 left_arm_motor_1,2,2,2,2,2,2,2,2,2
-right_leg_motor_0,0,0,0,0,0,0,0,0,0
-right_leg_motor_1,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4,0.1
-right_leg_motor_2,0.25,0.25,0.25,0.25,0.3,0.3,1.5,1,0.25
-right_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-1,-1,-1,-1,-0.4
-right_leg_motor_4,0.2,0.2,0.2,0.2,0.5,0.5,0,0,0.2
-right_leg_motor_5,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2,-0.1
+right_hip_yaw,0,0,0,0,0,0,0,0,0
+right_hip_roll,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4,0.1
+right_hip_pitch,0.25,0.25,0.25,0.25,0.3,0.3,1.5,1,0.25
+right_knee,-0.4,-0.4,-0.4,-0.4,-1,-1,-1,-1,-0.4
+right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,0.5,0,0,0.2
+right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2,-0.1
 right_arm_motor_0,0,0,0,0,1,1,-1,-1,0
 right_arm_motor_1,2,2,2,2,2,2,2,2,2
 comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance,stance

soccer_control/soccer_trajectories/trajectories/bez1/rightkick_old.csv

@@ -5,12 +5,12 @@ left_leg_motor_2,0.25,0.25,0.25,0.25,0.25,0.25
 left_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4
 left_leg_motor_4,0.2,0.2,0.2,0.2,0.2,0.2
 left_leg_motor_5,0.05,0.2,0.4,0.2,0.2,0.2
-right_leg_motor_0,0,0,0,0,0,0
-right_leg_motor_1,-0.05,-0.3,-1.5,-0.3,-0.3,-0.3
-right_leg_motor_2,0.25,0.4,0.7,-0.3,1.5,0.6
-right_leg_motor_3,-0.4,-0.5,-1.4,-2,-0.5,-1.4
-right_leg_motor_4,0.2,0.3,0.7,0.9,0.5,0.8
-right_leg_motor_5,0.05,0.3,0.3,0.2,0.2,0.2
+right_hip_yaw,0,0,0,0,0,0
+right_hip_roll,-0.05,-0.3,-1.5,-0.3,-0.3,-0.3
+right_hip_pitch,0.25,0.4,0.7,-0.3,1.5,0.6
+right_knee,-0.4,-0.5,-1.4,-2,-0.5,-1.4
+right_ankle_pitch,0.2,0.3,0.7,0.9,0.5,0.8
+right_ankle_roll,0.05,0.3,0.3,0.2,0.2,0.2
 right_arm_motor_0,0,0,0,0,0,0
 right_arm_motor_1,2,2,2,2,2,2
 left_arm_motor_0,0,0,0,0,0,0