def update_robot_ik(self):
prev_lf_task = self.robot.ik.tasks[self.robot.left_foot.name]
prev_rf_task = self.robot.ik.tasks[self.robot.right_foot.name]
contact_weight = max(prev_lf_task.weight, prev_rf_task.weight)
swing_weight = 1e-1
self.robot.ik.remove(self.robot.left_foot.name)
self.robot.ik.remove(self.robot.right_foot.name)
if self.cur_stance.left_foot is not None:
left_foot_task = ContactTask(
self.robot, self.robot.left_foot, self.cur_stance.left_foot,
weight=contact_weight)
else: # left_foot is swinging
left_foot_task = PoseTask(
self.robot, self.robot.left_foot, self.swing_foot,
weight=swing_weight)
if self.cur_stance.right_foot is not None:
right_foot_task = ContactTask(
self.robot, self.robot.right_foot, self.cur_stance.right_foot,
weight=contact_weight)
else: # right_foot is swingign
right_foot_task = PoseTask(
self.robot, self.robot.right_foot, self.swing_foot,
weight=swing_weight)
self.robot.ik.add(left_foot_task)
self.robot.ik.add(right_foot_task)
def switch_ik_tasks(self, phase):
"""Prepare robot IK for next step."""
assert phase in ['SS', 'DS']
prev_lf_task = self.robot.ik.get_task(self.robot.left_foot.name)
prev_rf_task = self.robot.ik.get_task(self.robot.right_foot.name)
contact_weight = max(prev_lf_task.weight, prev_rf_task.weight)
self.robot.ik.remove_task(self.robot.left_foot.name)
self.robot.ik.remove_task(self.robot.right_foot.name)
OtherTask = PoseTask if phase == 'SS' else ContactTask
other_weight = self.__min_swing_weight if phase == 'SS' else \
contact_weight
if 'left' in self.support_contact.link.name.lower():
left_foot_task = ContactTask(self.robot,
self.robot.left_foot,
self.support_contact,
weight=contact_weight)
right_foot_task = OtherTask(self.robot,
self.robot.right_foot,
self.swing_controller.foot,
weight=other_weight)
link_pose_task = right_foot_task
else: # support contact is on the right foot
left_foot_task = OtherTask(self.robot,
self.robot.left_foot,
self.swing_controller.foot,
weight=other_weight)
right_foot_task = ContactTask(self.robot,
self.robot.right_foot,
self.support_contact,
weight=contact_weight)
link_pose_task = left_foot_task
self.robot.ik.add_task(left_foot_task)
self.robot.ik.add_task(right_foot_task)
self.link_pose_task = link_pose_task
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 update_robot_ik(self):
self.robot.ik.remove_task(self.robot.left_foot.name)
self.robot.ik.remove_task(self.robot.right_foot.name)
if self.cur_stance.left_foot is not None:
left_foot_task = ContactTask(self.robot, self.robot.left_foot,
self.cur_stance.left_foot)
else: # left_foot is free
left_foot_task = LinkPoseTask(self.robot, self.robot.left_foot,
self.free_foot)
if self.cur_stance.right_foot is not None:
right_foot_task = ContactTask(self.robot, self.robot.right_foot,
self.cur_stance.right_foot)
else: # right_foot is free
right_foot_task = LinkPoseTask(self.robot, self.robot.right_foot,
self.free_foot)
self.robot.ik.add_task(left_foot_task)
self.robot.ik.add_task(right_foot_task)
def update_robot_ik():
with robot_lock:
robot.ik.remove_task(robot.left_foot.name)
robot.ik.remove_task(robot.right_foot.name)
if fsm.cur_stance.left_foot is not None:
left_foot_task = ContactTask(
robot, robot.left_foot, fsm.cur_stance.left_foot)
else: # left_foot is free
left_foot_task = LinkPoseTask(
robot, robot.left_foot, fsm.free_foot)
if fsm.cur_stance.right_foot is not None:
right_foot_task = ContactTask(
robot, robot.right_foot, fsm.cur_stance.right_foot)
else: # right_foot is free
right_foot_task = LinkPoseTask(
robot, robot.right_foot, fsm.free_foot)
robot.ik.add_task(left_foot_task)
robot.ik.add_task(right_foot_task)
def initialize_robot():
robot.set_dof_values(robot.q_halfsit)
active_dofs = robot.chest + robot.free
active_dofs += robot.left_leg + robot.right_leg
robot.set_active_dofs(active_dofs)
robot.init_ik(gains={
'com': 1.,
'contact': 1.,
'link_pose': 1.,
},
weights={
'com': 10.,
'contact': 1000.,
'link_pose': 100.,
})
robot.set_dof_values([1.], [robot.TRANS_Z]) # warm-start PG
stance = fsm.cur_stance
com_task = COMTask(robot, stance.com)
robot.ik.add_task(com_task)
robot.ik.add_task(ContactTask(robot, robot.left_foot, stance.left_foot))
robot.ik.add_task(ContactTask(robot, robot.right_foot, stance.right_foot))
robot.ik.add_task(MinVelocityTask(robot, weight=1.))
robot.solve_ik(max_it=50)
# active_dofs += [robot.L_ELBOW_P, robot.R_ELBOW_P]
active_dofs += [robot.L_SHOULDER_R, robot.R_SHOULDER_R]
robot.set_dof_limits([+0.15], [+1.], [robot.L_SHOULDER_R])
robot.set_dof_limits([-1.], [-0.15], [robot.R_SHOULDER_R])
robot.set_active_dofs(active_dofs)
com_task.update_target(preview_buffer.com)
robot.ik.add_task(MinCAMTask(robot, weight=1.))
robot.ik.add_task(DOFTask(robot, robot.CHEST_P, 0.2, gain=0.9,
weight=0.05))
robot.ik.add_task(DOFTask(robot, robot.CHEST_Y, 0., gain=0.9, weight=0.05))
robot.ik.add_task(DOFTask(robot, robot.ROT_P, 0., gain=0.9, weight=0.05))
robot.solve_ik()
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))
# First, generate an initial posture
robot.solve_ik(max_it=100, conv_tol=1e-4, debug=True)
# Next, we move the COM back and forth for 10 seconds
[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], visible=True)
rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0], visible=True)
# 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,
weight=1.,
gain=0.85)
com_task = COMTask(robot, com, weight=1e-2, gain=0.85)
reg_task = PostureTask(robot, robot.q, weight=1e-6, gain=0.85)
# IK setup
robot.ik.clear_tasks()
robot.ik.add_task(lf_task)
robot.ik.add_task(rf_task)
robot.ik.add_task(com_task)