def __init__(self, rm, dt):
self.qpsolver.addConstraintSet(self.dynamicsConstraint)
self.qpsolver.addConstraintSet(self.contactConstraint)
self.qpsolver.addTask(self.postureTask)
self.qpsolver.setContacts([
mc_rbdyn.Contact(self.robots(), 0, 2, "LeftFoot", "AllGround"),
mc_rbdyn.Contact(self.robots(), 0, 2, "RightFoot", "AllGround")
])
self.comTask = mc_tasks.CoMTask(self.robots(), 0, 10.0, 1000.0)
self.qpsolver.addTask(self.comTask)
self.postureTask.stiffness(1)
self.phase = APPROACH
def __init__(self, rm, dt):
self.qpsolver.addConstraintSet(self.dynamicsConstraint)
self.qpsolver.addConstraintSet(self.contactConstraint)
self.qpsolver.addTask(self.postureTask)
self.qpsolver.setContacts([
mc_rbdyn.Contact(self.robots(), 0, 1, "LeftFoot", "AllGround"),
mc_rbdyn.Contact(self.robots(), 0, 1, "RightFoot", "AllGround")
])
self.comTask = mc_tasks.CoMTask(self.robots(), 0, 10.0, 1000.0)
self.qpsolver.addTask(self.comTask)
self.postureTask.stiffness(1)
self.jointIndex = self.robot().jointIndexByName("NECK_Y")
self.goingLeft = True
self.comDown = True
self.comZero = eigen.Vector3d.Zero()
def __init__(self, rm, dt):
self.qpsolver.addConstraintSet(self.dynamicsConstraint)
self.qpsolver.addConstraintSet(self.contactConstraint)
self.qpsolver.addTask(self.postureTask)
self.positionTask = mc_tasks.PositionTask("R_WRIST_Y_S", self.robots(),
0)
self.qpsolver.addTask(self.positionTask)
self.qpsolver.setContacts([
mc_rbdyn.Contact(self.robots(), "LeftFoot", "AllGround"),
mc_rbdyn.Contact(self.robots(), "RightFoot", "AllGround")
])
self.hj1_name = "NECK_P"
self.hj1_index = self.robot().jointIndexByName(self.hj1_name)
self.hj1_q = 0.5
# Stuff to log
self.v3d_data = eigen.Vector3d.Random()
self.logger().addLogEntry("PYTHONV3D", lambda: self.v3d_data)
self.d_data = 0.0
self.dv_data = [self.d_data] * 3
self.theta = 0
self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
def switch_phase(self):
if self.phase == APPROACH and self.handTask.eval().norm(
) < 0.05 and self.handTask.speed().norm() < 1e-4:
# Add a new contact
contacts = self.qpsolver.contacts()
contacts.append(
mc_rbdyn.Contact(self.robots(), 0, 1, "RightGripper",
"Handle"))
self.qpsolver.setContacts(contacts)
# Remove the surface transform task
self.qpsolver.removeTask(self.handTask)
# Keep the robot in its current posture
self.postureTask.reset()
self.comTask.reset()
# Target new handle position
self.doorPosture.target({"handle": [-1.0]})
# Switch phase
self.phase = HANDLE
elif self.phase == HANDLE and self.doorPosture.eval().norm() < 0.01:
# Update door opening target
self.doorPosture.target({"door": [0.5]})
# Switch phase
self.phase = OPEN