def setup_ik_from_tasks():
"""
Setup the inverse kinematics task by task.
Note
-----
This function is equivalent to :func:`setup_ik_from_stance` above.
Beginners should take a look at that one first.
Notes
-----
See this `tutorial on inverse kinematics
<https://scaron.info/teaching/inverse-kinematics.html>`_ for details.
"""
from pymanoid.tasks import COMTask, ContactTask, DOFTask, PostureTask
# Prepare targets
lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0])
rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0])
# Initial robot pose
robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z])
com = PointMass(pos=robot.com, mass=robot.mass)
# Prepare tasks
left_foot_task = ContactTask(robot,
robot.left_foot,
lf_target,
weight=1.,
gain=0.85)
right_foot_task = ContactTask(robot,
robot.right_foot,
rf_target,
weight=1.,
gain=0.85)
com_task = COMTask(robot, com, weight=1e-2, gain=0.85)
posture_task = PostureTask(robot, robot.q_halfsit, weight=1e-6, gain=0.85)
# Add tasks to the IK solver
robot.ik.add(left_foot_task)
robot.ik.add(right_foot_task)
robot.ik.add(com_task)
robot.ik.add(posture_task)
# Add shoulder DOF tasks for a nicer posture
robot.ik.add(
DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5))
robot.ik.add(
DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5))
def generate_staircase(radius, angular_step, height, roughness, friction,
step_dim_x, step_dim_y):
"""
Generate a new slanted staircase with tilted steps.
INPUT:
- ``radius`` -- staircase radius (in [m])
- ``angular_step`` -- angular step between contacts (in [rad])
- ``height`` -- altitude variation (in [m])
- ``roughness`` -- amplitude of contact roll, pitch and yaw (in [rad])
- ``friction`` -- friction coefficient between a robot foot and a step
- ``step_dim_x`` -- half-length of each step
- ``step_dim_y`` -- half-width of each step
"""
steps = []
for theta in arange(0., 2 * pi, angular_step):
left_foot = Contact(
X=step_dim_x,
Y=step_dim_y,
pos=[radius * cos(theta),
radius * sin(theta),
radius + .5 * height * sin(theta)],
rpy=(roughness * (random(3) - 0.5) +
[0, 0, theta + .5 * pi]),
friction=friction,
visible=True)
right_foot = Contact(
X=step_dim_x,
Y=step_dim_y,
pos=[1.3 * radius * cos(theta + .5 * angular_step),
1.3 * radius * sin(theta + .5 * angular_step),
radius + .5 * height * sin(theta + .5 * angular_step)],
rpy=(roughness * (random(3) - 0.5) +
[0, 0, theta + .5 * pi]),
friction=friction,
visible=True)
steps.append(left_foot)
steps.append(right_foot)
return steps
def generate_contacts():
FOOT_X, FOOT_Y = 0.2, 0.1
dstep = 0.7 # [m]
friction = 0.9
height = 1.4 # [m]
leg_spread = 0.4 # [m]
nb_steps = 40
nb_waves = 2
rpy_roughness = 0.5 # [rad]
length = (nb_steps / 2) * dstep
steps = []
for x in arange(0., length, dstep):
theta = 2 * pi * nb_waves * x / length
left_foot = Contact(X=FOOT_X,
Y=FOOT_Y,
pos=[
x, leg_spread * (0.1 * sin(theta) + 0.5),
.5 * height * sin(theta)
],
rpy=rpy_roughness * (random(3) - 0.5),
friction=friction,
visible=True)
left_foot.set_pitch(sin(theta - pi / 2) / 2)
right_foot = Contact(X=FOOT_X,
Y=FOOT_Y,
pos=[
x + 0.5 * dstep,
leg_spread * (0.1 * sin(theta) - 0.5),
.5 * height * sin(theta + .5 * 0.5)
],
rpy=rpy_roughness * (random(3) - 0.5),
friction=friction,
visible=True)
right_foot.set_pitch(left_foot.pitch)
steps.append(left_foot)
steps.append(right_foot)
return steps
def generate_staircase(radius, angular_step, height, roughness, friction,
ds_duration, ss_duration, init_com_offset=None):
"""
Generate a new slanted staircase with tilted steps.
Parameters
----------
radius : scalar
Staircase radius in [m].
angular_step : scalar
Angular step between contacts in [rad].
height : scalar
Altitude variation in [m].
roughness : scalar
Amplitude of contact roll, pitch and yaw in [rad].
friction : scalar
Friction coefficient between a robot foot and a step.
ds_duration : scalar
Duration of double-support phases in [s].
ss_duration : scalar
Duration of single-support phases in [s].
init_com_offset : array, optional
Initial offset applied to first stance.
"""
stances = []
contact_shape = (0.12, 0.06)
first_left_foot = None
prev_right_foot = None
for theta in arange(0., 2 * pi, angular_step):
left_foot = Contact(
shape=contact_shape,
pos=[radius * cos(theta),
radius * sin(theta),
radius + .5 * height * sin(theta)],
rpy=(roughness * (random(3) - 0.5) + [0, 0, theta + .5 * pi]),
friction=friction)
if first_left_foot is None:
first_left_foot = left_foot
right_foot = Contact(
shape=contact_shape,
pos=[1.2 * radius * cos(theta + .5 * angular_step),
1.2 * radius * sin(theta + .5 * angular_step),
radius + .5 * height * sin(theta + .5 * angular_step)],
rpy=(roughness * (random(3) - 0.5) + [0, 0, theta + .5 * pi]),
friction=friction)
if prev_right_foot is not None:
com_target_pos = left_foot.p + [0., 0., JVRC1.leg_length]
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsl_stance = Stance(
com_target, left_foot=left_foot, right_foot=prev_right_foot)
dsl_stance.label = 'DS-L'
dsl_stance.duration = ds_duration
ssl_stance = Stance(com_target, left_foot=left_foot)
ssl_stance.label = 'SS-L'
ssl_stance.duration = ss_duration
dsl_stance.compute_static_equilibrium_polygon()
ssl_stance.compute_static_equilibrium_polygon()
stances.append(dsl_stance)
stances.append(ssl_stance)
com_target_pos = right_foot.p + [0., 0., JVRC1.leg_length]
if init_com_offset is not None:
com_target_pos += init_com_offset
init_com_offset = None
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsr_stance = Stance(
com_target, left_foot=left_foot, right_foot=right_foot)
dsr_stance.label = 'DS-R'
dsr_stance.duration = ds_duration
ssr_stance = Stance(com_target, right_foot=right_foot)
ssr_stance.label = 'SS-R'
ssr_stance.duration = ss_duration
dsr_stance.compute_static_equilibrium_polygon()
ssr_stance.compute_static_equilibrium_polygon()
stances.append(dsr_stance)
stances.append(ssr_stance)
prev_right_foot = right_foot
com_target_pos = first_left_foot.p + [0., 0., JVRC1.leg_length]
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsl_stance = Stance(
com_target, left_foot=first_left_foot, right_foot=prev_right_foot)
dsl_stance.label = 'DS-L'
dsl_stance.duration = ds_duration
ssl_stance = Stance(com_target, left_foot=first_left_foot)
ssl_stance.label = 'SS-L'
ssl_stance.duration = ss_duration
dsl_stance.compute_static_equilibrium_polygon()
ssl_stance.compute_static_equilibrium_polygon()
stances.append(dsl_stance)
stances.append(ssl_stance)
return stances
robot.set_transparency(0.4)
dof_targets = [ # will also be passed to the IK
(robot.R_SHOULDER_R, -.5),
(robot.L_SHOULDER_R, +.5)]
q_init = robot.q.copy()
for (dof_id, dof_ref) in dof_targets:
robot.set_dof_values([dof_ref], [dof_id])
q_init[dof_id] = dof_ref
robot.set_dof_values([-1], [robot.R_SHOULDER_P])
robot.set_dof_values([-1], [robot.L_SHOULDER_P])
robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z])
init_com = robot.com.copy()
# IK targets
com = Cube(0.05, pos=robot.com, color='g')
lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0], visible=True)
rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0], visible=True)
# IK tasks
lf_task = ContactTask(robot, robot.left_foot, lf_target, weight=1000)
rf_task = ContactTask(robot, robot.right_foot, rf_target, weight=1000)
com_task = COMTask(robot, com, weight=10)
reg_task = PostureTask(robot, robot.q, weight=0.1) # regularization task
# IK setup
robot.init_ik(active_dofs=robot.whole_body)
robot.ik.add_tasks([lf_task, rf_task, com_task, reg_task])
for (dof_id, dof_ref) in dof_targets:
robot.ik.add_task(
DOFTask(robot, dof_id, dof_ref, gain=0.5, weight=0.1))
rem_time = dt - (time.time() - loop_start)
if rem_time > 0:
time.sleep(rem_time)
if __name__ == '__main__':
sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([[-0.28985317, 0.40434422, -0.86746233, 2.73872042],
[0.95680251, 0.10095043, -0.2726499, 0.86080128],
[-0.02267371, -0.90901857, -0.41613837, 2.06654644],
[0., 0., 0., 1.]])
# IK targets
lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0])
rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0])
# Initial robot pose
robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z])
com = PointMass(pos=robot.com, mass=robot.mass)
# IK tasks
lf_task = ContactTask(robot,
robot.left_foot,
lf_target,
weight=1.,
gain=0.85)
rf_task = ContactTask(robot,
robot.right_foot,
rf_target,
if __name__ == "__main__":
sim = pymanoid.Simulation(dt=0.03)
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([[0.2753152, 0.29704774, -0.91431077, 2.89042521],
[0.96034717, -0.04146411, 0.27570643, -0.59789598],
[0.04398688, -0.95396193, -0.29668466, 1.65848958],
[0., 0., 0., 1.]])
robot.set_transparency(0.25)
stance = Stance(com=PointMass(pos=[0.1, 0.2, 0.9], mass=robot.mass),
left_foot=Contact(shape=robot.sole_shape,
friction=0.7,
pos=[0.3, 0.39, 0.10],
link=robot.left_foot,
rpy=[0.15, 0., -0.02]),
right_foot=Contact(shape=robot.sole_shape,
friction=0.7,
pos=[0., -0.15, 0.02],
link=robot.right_foot,
rpy=[-0.3, 0., 0.]),
right_hand=Contact(shape=(0.04, 0.04),
friction=0.5,
pos=[0.1, -0.5, 1.2],
rpy=[-1.57, 0., 0.]))
stance.bind(robot)
robot.ik.solve()
soft_contact = SoftContact(stance.right_hand)