-
Notifications
You must be signed in to change notification settings - Fork 0
/
humanoid.py
112 lines (85 loc) · 3.25 KB
/
humanoid.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
import time
import placo
import numpy as np
from ischedule import schedule, run_loop
from placo_utils.visualization import robot_viz, point_viz, robot_frame_viz
"""
Sigmaban humanoid is moving its legs while looking at a moving ball.
"""
# Loading the robot
robot = placo.HumanoidRobot("../models/sigmaban/")
# Placing the left foot in world origin
robot.set_T_world_frame("left_foot", np.eye(4))
robot.update_kinematics()
solver = placo.KinematicsSolver(robot)
# 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")
# Creating the viewer
viz = robot_viz(robot)
# Trunk
com_task = solver.add_com_task(np.array([0.0, 0.0, 0.3]))
com_task.configure("com", "soft", 1.0)
trunk_orientation_task = solver.add_orientation_task("trunk", np.eye(3))
trunk_orientation_task.configure("trunk_orientation", "soft", 1.0)
# Keep left and right foot on the floor
left_foot_task = solver.add_frame_task("left_foot", T_world_left)
left_foot_task.configure("left_foot", "soft", 1.0, 1.0)
right_foot_task = solver.add_frame_task("right_foot", T_world_right)
right_foot_task.configure("right_foot", "soft", 1.0, 1.0)
# Look at ball
look_at_ball = solver.add_axisalign_task(
"camera", np.array([0.0, 0.0, 1.0]), np.array([0.0, 0.0, 1.0])
)
look_at_ball.configure("look_ball", "soft", 1.0)
# Creating a very basic lateral swing and foot rise trajectory
left_foot_z_traj = placo.CubicSpline()
left_foot_z_traj.add_point(0.0, 0.0, 0.0)
left_foot_z_traj.add_point(0.5, 0.05, 0.0)
left_foot_z_traj.add_point(1.0, 0.0, 0.0)
left_foot_z_traj.add_point(2.0, 0.0, 0.0)
right_foot_z_traj = placo.CubicSpline()
right_foot_z_traj.add_point(0.0, 0.0, 0.0)
right_foot_z_traj.add_point(1.0, 0.0, 0.0)
right_foot_z_traj.add_point(1.5, 0.05, 0.0)
right_foot_z_traj.add_point(2.0, 0.0, 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)
solver.enable_joint_limits(True)
solver.enable_velocity_limits(True)
t = 0
dt = 0.01
last = 0
solver.dt = dt
start_t = time.time()
robot.update_kinematics()
@schedule(interval=dt)
def loop():
global t
# Updating the target
t_mod = t % 2.0
target = left_foot_task.position().target_world
target[2] = left_foot_z_traj.pos(t_mod)
left_foot_task.position().target_world = target
target = right_foot_task.position().target_world
target[2] = right_foot_z_traj.pos(t_mod)
right_foot_task.position().target_world = target
# Updating the com target with lateral sinusoidal trajectory
com_task.target_world = np.array([0.0, -np.sin(t * np.pi) * 0.05 - 0.05, 0.3])
# Looking at ball
ball = np.array([0.5 + np.cos(t) * 0.25, np.sin(t) * 0.7, 0.0])
camera_pos = robot.get_T_world_frame("camera")[:3, 3]
look_at_ball.targetAxis_world = ball - camera_pos
robot.update_kinematics()
solver.solve(True)
viz.display(robot.state.q)
robot_frame_viz(robot, "trunk")
robot_frame_viz(robot, "camera")
point_viz("com", robot.com_world(), radius=0.025, color=0xAAAAAA)
point_viz("ball", ball, radius=0.05, color=0xDDDDDD)
t += dt
run_loop()