def __init__(self, conf, viewer=pin.visualize.MeshcatVisualizer):
self.conf = conf
self.t_limit = False
self.j_limit = True
self.robot = tsid.RobotWrapper(conf.urdf, [conf.path],
pin.JointModelFreeFlyer(), False)
robot = self.robot
self.model = robot.model()
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
self.q0 = q = self.model.referenceConfigurations["half_sitting"]
self.q0[0] = q[0] + 0.003 + 0.6
self.q0[1] = q[1] - 0.001
# self.q0[2] += 0.84
q = self.q0
v = np.zeros(robot.nv)
assert self.model.existFrame(conf.rf_frame_name)
assert self.model.existFrame(conf.lf_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contact_Point = np.ones((3, 4)) * conf.lz
contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name,
contact_Point, conf.contactNormal, conf.mu,
conf.fMin, conf.fMax)
contactRF.setKp(conf.kp_contact * np.ones(6))
contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.RF = robot.model().getFrameId(conf.rf_frame_name)
H_rf_ref = robot.framePosition(data, self.RF)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2] - conf.lz
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
H_rf_ref = robot.framePosition(data, self.RF)
contactRF.setReference(H_rf_ref)
if conf.w_contact >= 0.0:
formulation.addRigidContact(contactRF, conf.w_forceRef,
conf.w_contact, 1)
else:
formulation.addRigidContact(contactRF, conf.w_forceRef)
contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name,
contact_Point, conf.contactNormal, conf.mu,
conf.fMin, conf.fMax)
contactLF.setKp(conf.kp_contact * np.ones(6))
contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.LF = robot.model().getFrameId(conf.lf_frame_name)
H_lf_ref = robot.framePosition(data, self.LF)
contactLF.setReference(H_lf_ref)
if conf.w_contact >= 0.0:
formulation.addRigidContact(contactLF, conf.w_forceRef,
conf.w_contact, 1)
else:
formulation.addRigidContact(contactLF, conf.w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * conf.gain_vector)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture * conf.gain_vector))
postureTask.mask(conf.masks_posture)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
orientationRootTask = tsid.TaskSE3Equality("task-orientation-root",
robot, 'root_joint')
mask = np.ones(6)
mask[0:3] = 0
orientationRootTask.setMask(mask)
orientationRootTask.setKp(conf.kp_rootOrientation * mask)
orientationRootTask.setKd(2.0 *
np.sqrt(conf.kp_rootOrientation * mask))
trajoriRoot = tsid.TrajectorySE3Constant("traj-ori",
pin.SE3().Identity())
formulation.addMotionTask(orientationRootTask, conf.w_rootOrientation,
1, 0.0)
self.amTask = tsid.TaskAMEquality("task-am", robot)
self.amTask.setKp(conf.kp_am * np.array([1., 1., 0.]))
self.amTask.setKd(2.0 * np.sqrt(conf.kp_am * np.array([1., 1., 0.])))
formulation.addMotionTask(self.amTask, conf.w_am, 1, 0.)
self.sampleAM = tsid.TrajectorySample(3)
self.amTask.setReference(self.sampleAM)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot,
self.conf.lf_frame_name)
self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref)
formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0)
self.rightFootTask = tsid.TaskSE3Equality("task-right-foot",
self.robot,
self.conf.rf_frame_name)
self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0)
if self.t_limit:
self.tau_max = conf.tau_max_scaling * robot.model(
).effortLimit[-robot.na:]
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds(
"task-actuation-bounds", robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if conf.w_torque_bounds > 0.0:
print("torque_limit")
formulation.addActuationTask(actuationBoundsTask,
conf.w_torque_bounds, 0, 0.0)
if self.j_limit:
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot,
conf.dt)
self.v_max = conf.v_max_scaling * robot.model(
).velocityLimit[-robot.na:]
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if conf.w_joint_bounds > 0.0:
print("joint_limit")
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds,
0, 0.0)
com_ref = robot.com(data)
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
q_ref[:12] = np.array([
0., 0., -0.4114, 0.8594, -0.448, -0.0017, 0., 0., -0.4114, 0.8594,
-0.448, -0.0017
])
self.trajPosture = tsid.TrajectoryEuclidianConstant(
"traj_joint", q_ref)
postureTask.setReference(self.trajPosture.computeNext())
orientationRootTask.setReference(trajoriRoot.computeNext())
self.sampleLF = self.trajLF.computeNext()
self.sample_LF_pos = self.sampleLF.pos()
self.sample_LF_vel = self.sampleLF.vel()
self.sample_LF_acc = self.sampleLF.acc()
self.sampleRF = self.trajRF.computeNext()
self.sample_RF_pos = self.sampleRF.pos()
self.sample_RF_vel = self.sampleRF.vel()
self.sample_RF_acc = self.sampleRF.acc()
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.comTask = comTask
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
if self.t_limit:
self.actuationBoundsTask = actuationBoundsTask
if self.j_limit:
self.jointBoundsTask = jointBoundsTask
self.orientationRootTask = orientationRootTask
self.formulation = formulation
self.q = q
self.v = v
self.contact_LF_active = True
self.contact_RF_active = True
if viewer:
self.robot_display = pin.RobotWrapper.BuildFromURDF(
conf.urdf, [conf.path], pin.JointModelFreeFlyer())
if viewer == pin.visualize.GepettoVisualizer:
import gepetto.corbaserver
launched = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(launched[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
self.viz = viewer(self.robot_display.model,
self.robot_display.collision_model,
self.robot_display.visual_model)
self.viz.initViewer(loadModel=True)
self.viz.displayCollisions(False)
self.viz.displayVisuals(True)
self.viz.display(q)
self.gui = self.viz.viewer.gui
# self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
# self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF')
elif viewer == pin.visualize.MeshcatVisualizer:
self.viz = viewer(self.robot_display.model,
self.robot_display.collision_model,
self.robot_display.visual_model)
self.viz.initViewer(loadModel=True)
self.viz.display(q)
def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None, robot=None, queue_qt = None):
"""
Generate the whole body motion corresponding to the given contactSequence
:param cs: Contact sequence containing the references,
it will only be modified if the end effector trajectories are not valid.
New references will be generated and added to the cs
:param fullBody: Required to compute collision free end effector trajectories
:param viewer: If provided, and the settings are enabled, display the end effector trajectories and the last step computed
:param robot: a tsid.RobotWrapper instance. If None, a new one is created from the urdf files defined in cfg
:param queue_qt: If not None, the joint trajectories are send to this multiprocessing.Queue during computation
The queue take a tuple: [q_t (a Curve object), ContactPhase (may be None), Bool (True mean that this is the
last trajectory of the motion)]
:return: a new ContactSequence object, containing the wholebody trajectories,
and the other trajectories computed from the wholebody motion request with cfg.IK_STORE_* and a robotWrapper instance
"""
# define nested functions used in control loop #
def appendJointsValues(first_iter_for_phase = False):
"""
Append the current q value to the current phase.q_t trajectory
:param first_iter_for_phase: if True, set the current phase.q_init value
and initialize a new Curve for phase.q_t
"""
if first_iter_for_phase:
phase.q_init = q
phase.q_t = piecewise(polynomial(q.reshape(-1,1), t, t))
#phase.root_t = piecewise_SE3(constantSE3curve(SE3FromConfig(q) ,t))
else:
phase.q_t.append(q, t)
#phase.root_t.append(SE3FromConfig(q), t)
if queue_qt:
queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves()-1),
phase.dq_t.curve_at_index(phase.dq_t.num_curves()-1),
None,
False])
def appendJointsDerivatives(first_iter_for_phase=False):
"""
Append the current v and dv value to the current phase.dq_t and phase.ddq_t trajectory
:param first_iter_for_phase: if True, initialize a new Curve for phase.dq_t and phase.ddq_t
"""
if first_iter_for_phase:
phase.dq_t = piecewise(polynomial(v.reshape(-1, 1), t, t))
phase.ddq_t = piecewise(polynomial(dv.reshape(-1, 1), t, t))
else:
phase.dq_t.append(v, t)
phase.ddq_t.append(dv, t)
def appendTorques(first_iter_for_phase = False):
"""
Append the current tau value to the current phase.tau_t trajectory
:param first_iter_for_phase: if True, initialize a new Curve for phase.tau_t
"""
tau = invdyn.getActuatorForces(sol)
if first_iter_for_phase:
phase.tau_t = piecewise(polynomial(tau.reshape(-1,1), t, t))
else:
phase.tau_t.append(tau, t)
def appendCentroidal(first_iter_for_phase = False):
"""
Compute the values of the CoM position, velocity, acceleration, the anuglar momentum and it's derivative
from the wholebody data and append them to the current phase trajectories
:param first_iter_for_phase: if True, set the initial values for the current phase
and initialize the centroidal trajectories
"""
pcom, vcom, acom = pinRobot.com(q, v, dv)
L = pinRobot.centroidalMomentum(q, v).angular
dL = pin.computeCentroidalMomentumTimeVariation(pinRobot.model, pinRobot.data, q, v, dv).angular
if first_iter_for_phase:
phase.c_init = pcom
phase.dc_init = vcom
phase.ddc_init = acom
phase.L_init = L
phase.dL_init = dL
phase.c_t = piecewise(polynomial(pcom.reshape(-1,1), t , t))
phase.dc_t = piecewise(polynomial(vcom.reshape(-1,1), t, t))
phase.ddc_t = piecewise(polynomial(acom.reshape(-1,1), t, t))
phase.L_t = piecewise(polynomial(L.reshape(-1,1), t, t))
phase.dL_t = piecewise(polynomial(dL.reshape(-1,1), t, t))
else:
phase.c_t.append(pcom, t)
phase.dc_t.append(vcom, t)
phase.ddc_t.append(acom, t)
phase.L_t.append(L, t)
phase.dL_t.append(dL, t)
def appendZMP(first_iter_for_phase = False):
"""
Compute the zmp from the current wholebody data and append it to the current phase
:param first_iter_for_phase: if True, initialize a new Curve for phase.zmp_t
"""
tau = pin.rnea(pinRobot.model, pinRobot.data, q, v, dv)
# tau without external forces, only used for the 6 first
# res.tau_t[:6,k_t] = tau[:6]
phi0 = pinRobot.data.oMi[1].act(Force(tau[:6]))
wrench = phi0.vector
zmp = shiftZMPtoFloorAltitude(cs, t, phi0, cfg.Robot)
if first_iter_for_phase:
phase.zmp_t = piecewise(polynomial(zmp.reshape(-1,1), t, t))
phase.wrench_t = piecewise(polynomial(wrench.reshape(-1,1), t, t))
else:
phase.zmp_t.append(zmp, t)
phase.wrench_t.append(wrench, t)
def appendEffectorsTraj(first_iter_for_phase = False):
"""
Append the current position of the effectors not in contact to the current phase trajectories
:param first_iter_for_phase: if True, initialize a new Curve for the phase effector trajectories
"""
if first_iter_for_phase and phase_prev:
for eeName in phase_prev.effectorsWithTrajectory():
if t > phase_prev.effectorTrajectory(eeName).max():
placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
phase_prev.effectorTrajectory(eeName).append(placement, t)
for eeName in phase.effectorsWithTrajectory():
placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
if first_iter_for_phase:
phase.addEffectorTrajectory(eeName, piecewise_SE3(constantSE3curve(placement, t)))
else:
phase.effectorTrajectory(eeName).append(placement, t)
def appendContactForcesTrajs(first_iter_for_phase = False):
"""
Append the current contact force value to the current phase, for all the effectors in contact
:param first_iter_for_phase: if True, initialize a new Curve for the phase contact trajectories
"""
for eeName in phase.effectorsInContact():
contact = dic_contacts[eeName]
if invdyn.checkContact(contact.name, sol):
contact_forces = invdyn.getContactForce(contact.name, sol)
contact_normal_force = np.array(contact.getNormalForce(contact_forces))
else:
logger.warning("invdyn check contact returned false while the reference contact is active !")
contact_normal_force = np.zeros(1)
if cfg.Robot.cType == "_3_DOF":
contact_forces = np.zeros(3)
else:
contact_forces = np.zeros(12)
if first_iter_for_phase:
phase.addContactForceTrajectory(eeName, piecewise(polynomial(contact_forces.reshape(-1,1), t, t)))
phase.addContactNormalForceTrajectory(eeName, piecewise(polynomial(contact_normal_force.reshape(1,1), t, t)))
else:
phase.contactForce(eeName).append(contact_forces, t)
phase.contactNormalForce(eeName).append(contact_normal_force.reshape(1), t)
def storeData(first_iter_for_phase = False):
"""
Append all the required data (selected in the configuration file) to the current ContactPhase
:param first_iter_for_phase: if True, set the initial values for the current phase
and correctly initiliaze empty trajectories for this phase
"""
if cfg.IK_store_joints_derivatives:
appendJointsDerivatives(first_iter_for_phase)
appendJointsValues(first_iter_for_phase)
if cfg.IK_store_joints_torque:
appendTorques(first_iter_for_phase)
if cfg.IK_store_centroidal:
appendCentroidal(first_iter_for_phase)
if cfg.IK_store_zmp:
appendZMP(first_iter_for_phase)
if cfg.IK_store_effector:
appendEffectorsTraj(first_iter_for_phase)
if cfg.IK_store_contact_forces:
appendContactForcesTrajs(first_iter_for_phase)
def printIntermediate():
"""
Print the current state: active contacts, tracking errors, computed joint acceleration and velocity
:return:
"""
if logger.isEnabledFor(logging.INFO):
print("Time %.3f" % (t))
for eeName, contact in dic_contacts.items():
if invdyn.checkContact(contact.name, sol):
f = invdyn.getContactForce(contact.name, sol)
print("\tnormal force %s: %.1f" % (contact.name.ljust(20, '.'), contact.getNormalForce(f)))
print("\ttracking err %s: %.3f" % (comTask.name.ljust(20, '.'), norm(comTask.position_error, 2)))
for eeName in phase.effectorsWithTrajectory():
task = dic_effectors_tasks[eeName]
error = task.position_error
if cfg.Robot.cType == "_3_DOF":
error = error[0:3]
print("\ttracking err %s: %.3f" % (task.name.ljust(20, '.'), norm(error, 2)))
print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))
def checkDiverge():
"""
Check if either the joint velocity or acceleration is over a treshold or is NaN, and raise an error
"""
if norm(dv) > 1e6 or norm(v) > 1e6:
logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
logger.error("/!\ ABORT : controler unstable at t = %f /!\ ", t)
logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
raise ValueError("ABORT : controler unstable at t = " + str(t))
if math.isnan(norm(dv)) or math.isnan(norm(v)):
logger.error("!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
logger.error("/!\ ABORT : nan at t = %f /!\ ", t)
logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
raise ValueError("ABORT : controler unstable at t = " + str(t))
def stopHere():
"""
Set the current data as final values for the current phase, resize the contact sequence if needed and return
:return: [The current ContactSequence, the RobotWrapper]
"""
setPreviousFinalValues(phase_prev, phase, cfg)
if cfg.WB_ABORT_WHEN_INVALID:
# cut the sequence up to the last phase
cs.resize(pid)
return cs, robot
elif cfg.WB_RETURN_INVALID:
# cut the sequence up to the current phase
cs.resize(pid+1)
return cs, robot
### End of nested functions definitions ###
if not viewer:
logger.warning("No viewer linked, cannot display end_effector trajectories.")
logger.warning("Start TSID ... ")
# copy the given contact sequence to keep it as reference :
cs = ContactSequence(cs_ref)
# delete all the 'reference' trajectories from result (to leave room for the real trajectories stored)
deleteAllTrajectories(cs)
# Create a robot wrapper
if robot is None or cfg.IK_store_centroidal or cfg.IK_store_zmp:
rp = RosPack()
package_name = cfg.Robot.packageName.split("/")[0]
package_path = rp.get_path(package_name)
urdf = package_path + cfg.Robot.packageName.lstrip(
package_name) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
logger.info("load robot : %s", urdf)
if robot is None:
logger.info("load robot : %s", urdf)
robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
else:
logger.info("Use given robot in tsid.")
logger.info("robot loaded in tsid.")
if cfg.IK_store_centroidal or cfg.IK_store_zmp:
logger.info("load pinocchio robot ...")
# FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them
pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, package_path, pin.JointModelFreeFlyer())
logger.info("pinocchio robot loaded.")
# get the selected end effector trajectory generation method
effectorMethod, effectorCanRetry = cfg.get_effector_method()
# get the selected simulator method
Simulator = cfg.get_simulator_class()
simulator = Simulator(cfg.IK_dt, robot.model())
### Define initial state of the robot ###
phase0 = cs.contactPhases[0]
q = phase0.q_init[:robot.nq].copy()
if not q.any():
raise RuntimeError("The contact sequence doesn't contain an initial whole body configuration")
t = phase0.timeInitial
if cs_ref.contactPhases[0].dq_t and cs_ref.contactPhases[0].dq_t.min() <= t <= cs_ref.contactPhases[0].dq_t.max():
v = cs_ref.contactPhases[0].dq_t(t)
else:
v = np.zeros(robot.nv)
logger.debug("V_init used in tsid : %s", v)
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
simulator.init(q, v)
# add initial contacts :
dic_contacts = {}
for eeName in cs.contactPhases[0].effectorsInContact():
# replace the initial contact patch placements if needed to match exactly the current position in the problem:
updateContactPlacement(cs, 0, eeName,
getCurrentEffectorPosition(robot, invdyn.data(), eeName),
cfg.Robot.cType == "_6_DOF")
# create the contacts :
contact = createContactForEffector(cfg, invdyn, robot, eeName, phase0.contactPatch(eeName), False)
dic_contacts.update({eeName: contact})
if cfg.EFF_CHECK_COLLISION: # initialise object needed to check the motion
from mlp.utils import check_path
validator = check_path.PathChecker(fullBody, cfg.CHECK_DT, logger.isEnabledFor(logging.INFO))
### Initialize all task used ###
logger.info("initialize tasks : ")
if cfg.w_com > 0. :
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(cfg.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(cfg.kp_com) * np.ones(3))
invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0)
else:
comTask = None
if cfg.w_am > 0.:
amTask = tsid.TaskAMEquality("task-am", robot)
amTask.setKp(cfg.kp_am * np.array([1., 1., 0.]))
amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.array([1., 1., 0.])))
invdyn.addMotionTask(amTask, cfg.w_am, cfg.level_am, 0.)
else:
amTask = None
if cfg.w_posture > 0.:
postureTask = tsid.TaskJointPosture("task-joint-posture", robot)
postureTask.setKp(cfg.kp_posture * cfg.gain_vector)
postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector))
postureTask.setMask(cfg.masks_posture)
invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0)
q_ref = cfg.IK_REFERENCE_CONFIG
samplePosture = tsid.TrajectorySample(q_ref.shape[0] - 7)
samplePosture.pos(q_ref[7:]) # -7 because we remove the freeflyer part
else :
postureTask = None
if cfg.w_rootOrientation > 0. :
orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot, 'root_joint')
mask = np.ones(6)
mask[0:3] = 0
mask[5] = cfg.YAW_ROT_GAIN
orientationRootTask.setMask(mask)
orientationRootTask.setKp(cfg.kp_rootOrientation * mask)
orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask))
invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation, cfg.level_rootOrientation, 0.0)
else:
orientationRootTask = None
# init effector task objects :
usedEffectors = cs.getAllEffectorsInContact()
dic_effectors_tasks = createEffectorTasksDic(cfg, usedEffectors, robot)
# Add bounds tasks if required:
if cfg.w_torque_bounds > 0.:
tau_max = cfg.scaling_torque_bounds*robot.model().effortLimit[-robot.na:]
tau_min = -tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(tau_min, tau_max)
invdyn.addActuationTask(actuationBoundsTask, cfg.w_torque_bounds, 0, 0.0)
if cfg.w_joint_bounds > 0.:
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, cfg.IK_dt)
v_max = cfg.scaling_vel_bounds * robot.model().velocityLimit[-robot.na:]
v_min = -v_max
print("v_max : ", v_max)
jointBoundsTask.setVelocityBounds(v_min, v_max)
invdyn.addMotionTask(jointBoundsTask, cfg.w_joint_bounds, 0, 0.0)
solver = tsid.SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
# time check
dt = cfg.IK_dt
logger.info("dt : %f", dt)
logger.info("tsid initialized, start control loop")
#raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)")
time_start = time.time()
# For each phases, create the necessary task and references trajectories :
for pid in range(cs.size()):
logger.info("## for phase : %d", pid)
logger.info("t = %f", t)
# phase_ref contains the reference trajectories and should not be modified exept for new effector trajectories
# when the first ones was not collision free
phase_ref = cs_ref.contactPhases[pid]
# phase de not contains trajectories (exept for the end effectors) and should be modified with the new values computed
phase = cs.contactPhases[pid]
if pid > 0:
phase_prev = cs.contactPhases[pid - 1]
else:
phase_prev = None
if pid < cs.size() - 1:
phase_next = cs.contactPhases[pid + 1]
else:
phase_next = None
time_interval = [phase_ref.timeInitial, phase_ref.timeFinal]
logger.info("time_interval %s", time_interval)
# take CoM and AM trajectory from the phase, with their derivatives
com_traj = [phase_ref.c_t, phase_ref.dc_t, phase_ref.ddc_t]
am_traj = [phase_ref.L_t, phase_ref.L_t, phase_ref.dL_t]
# add root's orientation ref from reference config :
root_traj = phase_ref.root_t
# add se3 tasks for end effector when required
for eeName in phase.effectorsWithTrajectory():
logger.info("add se3 task for %s", eeName)
task = dic_effectors_tasks[eeName]
invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.)
adjustEndEffectorTrajectoryIfNeeded(cfg, phase_ref, robot, invdyn.data(), eeName, effectorMethod)
logger.info("t interval : %s", time_interval)
# add newly created contacts :
new_contacts_names = [] # will store the names of the contact tasks created at this phase
for eeName in usedEffectors:
if phase_prev and phase_ref.isEffectorInContact(eeName) and not phase_prev.isEffectorInContact(eeName):
invdyn.removeTask(dic_effectors_tasks[eeName].name, 0.0) # remove pin task for this contact
logger.info("remove se3 effector task : %s", dic_effectors_tasks[eeName].name)
if logger.isEnabledFor(logging.DEBUG):
current_placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
logger.debug("Current effector placement : %s", current_placement)
logger.debug("Reference effector placement : %s", cs.contactPhases[pid].contactPatch(eeName).placement)
updateContactPlacement(cs, pid, eeName,
getCurrentEffectorPosition(robot, invdyn.data(), eeName),
cfg.Robot.cType == "_6_DOF")
contact = createContactForEffector(cfg, invdyn, robot, eeName, phase.contactPatch(eeName))
new_contacts_names += [contact.name]
dic_contacts.update({eeName: contact})
logger.info("Create contact for : %s", eeName)
# start removing the contact that will be broken in the next phase :
# (This tell the solver that it should start minimizing the contact force on this contact, and ideally get to 0 at the given time)
for eeName, contact in dic_contacts.items():
if phase_next and phase.isEffectorInContact(eeName) and not phase_next.isEffectorInContact(eeName):
transition_time = phase.duration + dt/2.
logger.info("\nTime %.3f Start breaking contact %s. transition time : %.3f\n",
t, contact.name, transition_time)
exist = invdyn.removeRigidContact(contact.name, transition_time)
assert exist, "Try to remove a non existing contact !"
# Remove all effectors not in contact at this phase,
# This is required as the block above may not remove the contact exactly at the desired time
# FIXME: why is it required ? Numerical approximation in the transition_time ?
for eeName, contact in dic_contacts.items():
if not phase.isEffectorInContact(eeName):
exist = invdyn.removeRigidContact(contact.name, 0.)
if exist:
logger.warning("Contact "+eeName+" was not remove after the given transition time.")
if cfg.WB_STOP_AT_EACH_PHASE:
input('start simulation')
# save values at the beginning of the current phase
q_begin = q.copy()
v_begin = v.copy()
phase.q_init = q_begin
if phase_prev:
phase_prev.q_final = q_begin
phaseValid = False
iter_for_phase = -1
# iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits)
while not phaseValid:
deletePhaseWBtrajectories(phase) # clean previous invalid trajectories
t = phase.timeInitial
k_t = 0
if iter_for_phase >= 0:
# reset values to their value at the beginning of the current phase
q = q_begin.copy()
v = v_begin.copy()
simulator.q = q
simulator.v = v
iter_for_phase += 1
logger.info("Start computation for phase %d , t = %f, try number : %d", pid, t, iter_for_phase)
# loop to generate states (q,v,a) for the current contact phase :
while t < phase.timeFinal - (dt / 2.):
# set traj reference for current time :
# com
if comTask:
sampleCom = curvesToTSID(com_traj,t)
comTask.setReference(sampleCom)
# am
if amTask:
if cfg.IK_trackAM:
sampleAM = curvesToTSID(am_traj,t)
else:
sampleAM = tsid.TrajectorySample(3)
amTask.setReference(sampleAM)
# posture
#print "postural task ref : ",samplePosture.pos()
if postureTask:
postureTask.setReference(samplePosture)
# root orientation :
if orientationRootTask:
sampleRoot = curveSE3toTSID(root_traj,t)
orientationRootTask.setReference(sampleRoot)
# update weight of regularization tasks for the new contacts:
if len(new_contacts_names) > 0 :
# linearly decrease the weight of the tasks for the newly created contacts
u_w_force = (t - phase.timeInitial) / (phase.duration * cfg.w_forceRef_time_ratio)
if u_w_force <= 1.:
current_w_force = cfg.w_forceRef_init * (1. - u_w_force) + cfg.w_forceRef_end * u_w_force
for contact_name in new_contacts_names:
success = invdyn.updateRigidContactWeights(contact_name, current_w_force)
assert success, "Unable to change the weight of the force regularization task for contact "+contact_name
logger.debug("### references given : ###")
logger.debug("com pos : %s", sampleCom.pos())
logger.debug("com vel : %s", sampleCom.vel())
logger.debug("com acc : %s", sampleCom.acc())
if amTask:
logger.debug("AM pos : %s", sampleAM.pos())
logger.debug("AM vel : %s", sampleAM.vel())
logger.debug("root pos : %s", sampleRoot.pos())
logger.debug("root vel : %s", sampleRoot.vel())
# end effector (if they exists)
for eeName, traj in phase_ref.effectorTrajectories().items():
sampleEff = curveSE3toTSID(traj,t,True)
dic_effectors_tasks[eeName].setReference(sampleEff)
logger.debug("effector %s, pos = %s", eeName, sampleEff.pos())
logger.debug("effector %s, vel = %s", eeName, sampleEff.vel())
logger.debug("previous q = %s", q)
logger.debug("previous v = %s", v)
# solve HQP for the current time
HQPData = invdyn.computeProblemData(t, q, v)
if t < phase.timeInitial + dt:
logger.info("final data for phase %d", pid)
if logger.isEnabledFor(logging.INFO):
HQPData.print_all()
sol = solver.solve(HQPData)
dv = invdyn.getAccelerations(sol)
storeData(k_t == 0)
q, v = simulator.simulate(dv)
t += dt
k_t += 1
if t >= phase.timeFinal - (dt / 2.):
t = phase.timeFinal # avoid numerical imprecisions
logger.debug("v = %s", v)
logger.debug("dv = %s", dv)
if int(t / dt) % cfg.IK_PRINT_N == 0:
printIntermediate()
try:
checkDiverge()
except ValueError:
return stopHere()
# end while t \in phase_t (loop for the current contact phase)
if len(phase.effectorsWithTrajectory()) > 0 and cfg.EFF_CHECK_COLLISION:
phaseValid, t_invalid = validator.check_motion(phase.q_t)
#if iter_for_phase < 3 :# debug only, force limb-rrt
# phaseValid = False
# t_invalid = (phase.timeInitial + phase.timeFinal) / 2.
if not phaseValid:
if iter_for_phase == 0:
# save the first q_t trajectory computed, for limb-rrt
first_q_t = phase.q_t
logger.warning("Phase %d not valid at t = %f", pid, t_invalid)
logger.info("First invalid q : %s", phase.q_t(t_invalid))
if t_invalid <= (phase.timeInitial + cfg.EFF_T_PREDEF) \
or t_invalid >= (phase.timeFinal - cfg.EFF_T_PREDEF):
logger.error("Motion is invalid during predefined phases, cannot change this.")
return stopHere()
if effectorCanRetry():
logger.warning("Try new end effector trajectory.")
try:
for eeName, ref_traj in phase_ref.effectorTrajectories().items():
placement_init = ref_traj.evaluateAsSE3(phase.timeInitial)
placement_end = ref_traj.evaluateAsSE3(phase.timeFinal)
traj = effectorMethod(cfg, time_interval, placement_init, placement_end,
iter_for_phase + 1, first_q_t, phase_prev,
phase_ref, phase_next, fullBody, eeName, viewer)
# save the new trajectory in the phase with the references
phase_ref.addEffectorTrajectory(eeName,traj)
if cfg.DISPLAY_ALL_FEET_TRAJ and logger.isEnabledFor(logging.INFO):
color = fullBody.dict_limb_color_traj[eeName]
color[-1] = 0.6 # set small transparency
display_tools.displaySE3Traj(phase_ref.effectorTrajectory(eeName),
viewer.client.gui,
viewer.sceneName,
eeName + "_traj_" + str(pid) + "_" + str(iter_for_phase),
color,
[phase.timeInitial, phase.timeFinal],
fullBody.dict_offset[eeName])
except ValueError as e:
logging.error("ERROR in generateEndEffectorTraj :", exc_info=e)
return stopHere()
else:
logging.error("End effector method choosen do not allow retries, abort here.")
return stopHere()
else: # no effector motions, phase always valid (or bypass the check)
phaseValid = True
logging.info("Phase %d valid.", pid)
if phaseValid:
setPreviousFinalValues(phase_prev, phase, cfg)
# display the progress by moving the robot at the last configuration computed
if viewer and cfg.IK_SHOW_PROGRESS:
display_tools.displayWBconfig(viewer,q)
#end while not phaseValid
# end for all phases
# store the data of the last point
phase_prev = phase
phase = ContactPhase(phase_prev)
storeData(True)
setPreviousFinalValues(phase_prev, phase, cfg)
time_end = time.time() - time_start
logger.warning("Whole body motion generated in : %f s.", time_end)
logger.info("\nFinal COM Position %s", robot.com(invdyn.data()))
logger.info("Desired COM Position %s", cs.contactPhases[-1].c_final)
if queue_qt:
queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves() - 1), None, True])
return cs, robot
def generateWholeBodyMotion(cs, fullBody=None, viewer=None):
if not viewer:
print "No viewer linked, cannot display end_effector trajectories."
print "Start TSID ... "
rp = RosPack()
urdf = rp.get_path(
cfg.Robot.packageName
) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
if cfg.WB_VERBOSE:
print "load robot : ", urdf
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
if cfg.WB_VERBOSE:
print "robot loaded in tsid."
# FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them
pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
q = cs.contact_phases[0].reference_configurations[0][:robot.nq].copy()
v = np.matrix(np.zeros(robot.nv)).transpose()
t = 0.0 # time
# init states list with initial state (assume joint velocity is null for t=0)
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
if cfg.EFF_CHECK_COLLISION: # initialise object needed to check the motion
from mlp.utils import check_path
validator = check_path.PathChecker(fullBody, cs, len(q),
cfg.WB_VERBOSE)
if cfg.WB_VERBOSE:
print "initialize tasks : "
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(cfg.kp_com * np.matrix(np.ones(3)).transpose())
comTask.setKd(2.0 * np.sqrt(cfg.kp_com) *
np.matrix(np.ones(3)).transpose())
invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0)
com_ref = robot.com(invdyn.data())
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
amTask = tsid.TaskAMEquality("task-am", robot)
amTask.setKp(cfg.kp_am * np.matrix([1., 1., 0.]).T)
amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.matrix([1., 1., 0.]).T))
invdyn.addTask(amTask, cfg.w_am, cfg.level_am)
trajAM = tsid.TrajectoryEuclidianConstant("traj_am",
np.matrix(np.zeros(3)).T)
postureTask = tsid.TaskJointPosture("task-joint-posture", robot)
postureTask.setKp(cfg.kp_posture * cfg.gain_vector)
postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector))
postureTask.mask(cfg.masks_posture)
invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0)
q_ref = cfg.IK_REFERENCE_CONFIG
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref[7:])
orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot,
'root_joint')
mask = np.matrix(np.ones(6)).transpose()
mask[0:3] = 0
mask[5] = cfg.YAW_ROT_GAIN
orientationRootTask.setKp(cfg.kp_rootOrientation * mask)
orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask))
invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation,
cfg.level_rootOrientation, 0.0)
root_ref = robot.position(data, robot.model().getJointId('root_joint'))
trajRoot = tsid.TrajectorySE3Constant("traj-root", root_ref)
usedEffectors = []
for eeName in cfg.Robot.dict_limb_joint.values():
if isContactEverActive(cs, eeName):
usedEffectors.append(eeName)
# init effector task objects :
dic_effectors_tasks = createEffectorTasksDic(cs, robot)
effectorTraj = tsid.TrajectorySE3Constant(
"traj-effector", SE3.Identity()
) # trajectory doesn't matter as it's only used to get the correct struct and size
# init empty dic to store effectors trajectories :
dic_effectors_trajs = {}
for eeName in usedEffectors:
dic_effectors_trajs.update({eeName: None})
# add initial contacts :
dic_contacts = {}
for eeName in usedEffectors:
if isContactActive(cs.contact_phases[0], eeName):
contact = createContactForEffector(invdyn, robot,
cs.contact_phases[0], eeName)
dic_contacts.update({eeName: contact})
if cfg.PLOT: # init a dict storing all the reference trajectories used (for plotting)
stored_effectors_ref = {}
for eeName in dic_effectors_tasks:
stored_effectors_ref.update({eeName: []})
solver = tsid.SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
# define nested function used in control loop
def storeData(k_t, res, q, v, dv, invdyn, sol):
# store current state
res.q_t[:, k_t] = q
res.dq_t[:, k_t] = v
res.ddq_t[:, k_t] = dv
res.tau_t[:, k_t] = invdyn.getActuatorForces(
sol) # actuator forces, with external forces (contact forces)
#store contact info (force and status)
if cfg.IK_store_contact_forces:
for eeName, contact in dic_contacts.iteritems():
if invdyn.checkContact(contact.name, sol):
res.contact_forces[eeName][:,
k_t] = invdyn.getContactForce(
contact.name, sol)
res.contact_normal_force[
eeName][:, k_t] = contact.getNormalForce(
res.contact_forces[eeName][:, k_t])
res.contact_activity[eeName][:, k_t] = 1
# store centroidal info (real one and reference) :
if cfg.IK_store_centroidal:
pcom, vcom, acom = pinRobot.com(q, v, dv)
res.c_t[:, k_t] = pcom
res.dc_t[:, k_t] = vcom
res.ddc_t[:, k_t] = acom
res.L_t[:, k_t] = pinRobot.centroidalMomentum(q, v).angular
#res.dL_t[:,k_t] = pinRobot.centroidalMomentumVariation(q,v,dv) # FIXME : in robot wrapper, use * instead of .dot() for np matrices
pin.dccrba(pinRobot.model, pinRobot.data, q, v)
res.dL_t[:, k_t] = Force(
pinRobot.data.Ag.dot(dv) + pinRobot.data.dAg.dot(v)).angular
# same for reference data :
res.c_reference[:, k_t] = com_desired
res.dc_reference[:, k_t] = vcom_desired
res.ddc_reference[:, k_t] = acom_desired
res.L_reference[:, k_t] = L_desired
res.dL_reference[:, k_t] = dL_desired
if cfg.IK_store_zmp:
tau = pin.rnea(
pinRobot.model, pinRobot.data, q, v, dv
) # tau without external forces, only used for the 6 first
#res.tau_t[:6,k_t] = tau[:6]
phi0 = pinRobot.data.oMi[1].act(Force(tau[:6]))
res.wrench_t[:, k_t] = phi0.vector
res.zmp_t[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], phi0, cfg.EXPORT_OPENHRP)
# same but from the 'reference' values :
Mcom = SE3.Identity()
Mcom.translation = com_desired
Fcom = Force.Zero()
Fcom.linear = cfg.MASS * (acom_desired - cfg.GRAVITY)
Fcom.angular = dL_desired
F0 = Mcom.act(Fcom)
res.wrench_reference[:, k_t] = F0.vector
res.zmp_reference[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], F0, cfg.EXPORT_OPENHRP)
if cfg.IK_store_effector:
for eeName in usedEffectors: # real position (not reference)
res.effector_trajectories[eeName][:, k_t] = SE3toVec(
getCurrentEffectorPosition(robot, invdyn.data(), eeName))
res.d_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorVelocity(robot, invdyn.data(), eeName))
res.dd_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorAcceleration(robot, invdyn.data(),
eeName))
return res
def printIntermediate(v, dv, invdyn, sol):
print "Time %.3f" % (t)
for eeName, contact in dic_contacts.iteritems():
if invdyn.checkContact(contact.name, sol):
f = invdyn.getContactForce(contact.name, sol)
print "\tnormal force %s: %.1f" % (contact.name.ljust(
20, '.'), contact.getNormalForce(f))
print "\ttracking err %s: %.3f" % (comTask.name.ljust(
20, '.'), norm(comTask.position_error, 2))
for eeName, traj in dic_effectors_trajs.iteritems():
if traj:
task = dic_effectors_tasks[eeName]
error = task.position_error
if cfg.Robot.cType == "_3_DOF":
error = error[0:3]
print "\ttracking err %s: %.3f" % (task.name.ljust(
20, '.'), norm(error, 2))
print "\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv))
def checkDiverge(res, v, dv):
if norm(dv) > 1e6 or norm(v) > 1e6:
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "/!\ ABORT : controler unstable at t = " + str(t) + " /!\ "
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
raise ValueError("ABORT : controler unstable at t = " + str(t))
if math.isnan(norm(dv)) or math.isnan(norm(v)):
print "!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "/!\ ABORT : nan at t = " + str(t) + " /!\ "
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
raise ValueError("ABORT : controler unstable at t = " + str(t))
def stopHere():
if cfg.WB_ABORT_WHEN_INVALID:
return res.resize(phase_interval[0]), pinRobot
elif cfg.WB_RETURN_INVALID:
return res.resize(k_t), pinRobot
# time check
dt = cfg.IK_dt
if cfg.WB_VERBOSE:
print "dt : ", dt
res = Result(robot.nq, robot.nv, cfg.IK_dt, eeNames=usedEffectors, cs=cs)
N = res.N
last_display = 0
if cfg.WB_VERBOSE:
print "tsid initialized, start control loop"
#raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)")
time_start = time.time()
# For each phases, create the necessary task and references trajectories :
for pid in range(cs.size()):
if cfg.WB_VERBOSE:
print "## for phase : ", pid
print "t = ", t
phase = cs.contact_phases[pid]
if pid < cs.size() - 1:
phase_next = cs.contact_phases[pid + 1]
else:
phase_next = None
if pid > 0:
phase_prev = cs.contact_phases[pid - 1]
else:
phase_prev = None
t_phase_begin = res.phases_intervals[pid][0] * dt
t_phase_end = res.phases_intervals[pid][-1] * dt
time_interval = [t_phase_begin, t_phase_end]
if cfg.WB_VERBOSE:
print "time_interval ", time_interval
# generate com ref traj from phase :
com_init = np.matrix(np.zeros((9, 1)))
com_init[0:3, 0] = robot.com(invdyn.data())
com_traj = computeCOMRefFromPhase(phase, time_interval)
am_traj = computeAMRefFromPhase(phase, time_interval)
# add root's orientation ref from reference config :
if cfg.USE_PLANNING_ROOT_ORIENTATION:
if phase_next:
root_traj = trajectories.TrajectorySE3LinearInterp(
SE3FromConfig(phase.reference_configurations[0]),
SE3FromConfig(phase_next.reference_configurations[0]),
time_interval)
else:
root_traj = trajectories.TrajectorySE3LinearInterp(
SE3FromConfig(phase.reference_configurations[0]),
SE3FromConfig(phase.reference_configurations[0]),
time_interval)
else:
# orientation such that the torso orientation is the mean between both feet yaw rotations:
placement_init, placement_end = rootOrientationFromFeetPlacement(
phase, phase_next)
root_traj = trajectories.TrajectorySE3LinearInterp(
placement_init, placement_end, time_interval)
# add newly created contacts :
for eeName in usedEffectors:
if phase_prev and not isContactActive(
phase_prev, eeName) and isContactActive(phase, eeName):
invdyn.removeTask(dic_effectors_tasks[eeName].name,
0.0) # remove pin task for this contact
dic_effectors_trajs.update(
{eeName:
None}) # delete reference trajectory for this task
if cfg.WB_VERBOSE:
print "remove se3 task : " + dic_effectors_tasks[
eeName].name
contact = createContactForEffector(invdyn, robot, phase,
eeName)
dic_contacts.update({eeName: contact})
# add se3 tasks for end effector not in contact that will be in contact next phase:
for eeName, task in dic_effectors_tasks.iteritems():
if phase_next and not isContactActive(
phase, eeName) and isContactActive(phase_next, eeName):
if cfg.WB_VERBOSE:
print "add se3 task for " + eeName
invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.0)
#create reference trajectory for this task :
placement_init = getCurrentEffectorPosition(
robot, invdyn.data(), eeName
) #FIXME : adjust orientation in case of 3D contact ...
if cfg.Robot.cType == "_3_DOF":
placement_init.rotation = JointPlacementForEffector(
phase, eeName).rotation
placement_end = JointPlacementForEffector(phase_next, eeName)
ref_traj = generateEndEffectorTraj(time_interval,
placement_init,
placement_end, 0)
if cfg.WB_VERBOSE:
print "t interval : ", time_interval
print "placement Init : ", placement_init
print "placement End : ", placement_end
# display all the effector trajectories for this phase
if viewer and cfg.DISPLAY_ALL_FEET_TRAJ:
display_tools.displaySE3Traj(
ref_traj, viewer.client.gui, viewer.sceneName,
eeName + "_traj_" + str(pid),
cfg.Robot.dict_limb_color_traj[eeName], time_interval,
cfg.Robot.dict_offset[eeName])
viewer.client.gui.setVisibility(
eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP')
dic_effectors_trajs.update({eeName: ref_traj})
# start removing the contact that will be broken in the next phase :
# (This tell the solver that it should start minimzing the contact force on this contact, and ideally get to 0 at the given time)
for eeName, contact in dic_contacts.iteritems():
if phase_next and isContactActive(
phase, eeName) and not isContactActive(phase_next, eeName):
transition_time = t_phase_end - t - dt / 2.
if cfg.WB_VERBOSE:
print "\nTime %.3f Start breaking contact %s. transition time : %.3f\n" % (
t, contact.name, transition_time)
invdyn.removeRigidContact(contact.name, transition_time)
if cfg.WB_STOP_AT_EACH_PHASE:
raw_input('start simulation')
# save values at the beginning of the current phase
q_begin = q.copy()
v_begin = v.copy()
phaseValid = False
swingPhase = False # will be true if an effector move during this phase
iter_for_phase = -1
# iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits)
while not phaseValid:
if iter_for_phase >= 0:
# reset values to their value at the beginning of the current phase
q = q_begin.copy()
v = v_begin.copy()
iter_for_phase += 1
if cfg.WB_VERBOSE:
print "Start simulation for phase " + str(
pid) + ", try number : " + str(iter_for_phase)
# loop to generate states (q,v,a) for the current contact phase :
if pid == cs.size() - 1: # last state
phase_interval = res.phases_intervals[pid]
else:
phase_interval = res.phases_intervals[pid][:-1]
for k_t in phase_interval:
t = res.t_t[k_t]
# set traj reference for current time :
# com
sampleCom = trajCom.computeNext()
com_desired = com_traj(t)[0]
vcom_desired = com_traj(t)[1]
acom_desired = com_traj(t)[2]
sampleCom.pos(com_desired)
sampleCom.vel(vcom_desired)
sampleCom.acc(acom_desired)
comTask.setReference(sampleCom)
# am
sampleAM = trajAM.computeNext()
L_desired = am_traj(t)[0]
dL_desired = am_traj(t)[1]
sampleAM.pos(L_desired)
sampleAM.vel(dL_desired)
amTask.setReference(sampleAM)
# posture
samplePosture = trajPosture.computeNext()
#print "postural task ref : ",samplePosture.pos()
postureTask.setReference(samplePosture)
# root orientation :
sampleRoot = trajRoot.computeNext()
sampleRoot.pos(SE3toVec(root_traj(t)[0]))
sampleRoot.vel(MotiontoVec(root_traj(t)[1]))
orientationRootTask.setReference(sampleRoot)
quat_waist = Quaternion(root_traj(t)[0].rotation)
res.waist_orientation_reference[:, k_t] = np.matrix(
[quat_waist.x, quat_waist.y, quat_waist.z, quat_waist.w]).T
if cfg.WB_VERBOSE == 2:
print "### references given : ###"
print "com pos : ", sampleCom.pos()
print "com vel : ", sampleCom.vel()
print "com acc : ", sampleCom.acc()
print "AM pos : ", sampleAM.pos()
print "AM vel : ", sampleAM.vel()
print "root pos : ", sampleRoot.pos()
print "root vel : ", sampleRoot.vel()
# end effector (if they exists)
for eeName, traj in dic_effectors_trajs.iteritems():
if traj:
swingPhase = True # there is an effector motion in this phase
sampleEff = effectorTraj.computeNext()
traj_t = traj(t)
sampleEff.pos(SE3toVec(traj_t[0]))
sampleEff.vel(MotiontoVec(traj_t[1]))
dic_effectors_tasks[eeName].setReference(sampleEff)
if cfg.WB_VERBOSE == 2:
print "effector " + str(eeName) + " pos : " + str(
sampleEff.pos())
print "effector " + str(eeName) + " vel : " + str(
sampleEff.vel())
if cfg.IK_store_effector:
res.effector_references[eeName][:, k_t] = SE3toVec(
traj_t[0])
res.d_effector_references[
eeName][:, k_t] = MotiontoVec(traj_t[1])
res.dd_effector_references[
eeName][:, k_t] = MotiontoVec(traj_t[2])
elif cfg.IK_store_effector:
if k_t == 0:
res.effector_references[eeName][:, k_t] = SE3toVec(
getCurrentEffectorPosition(
robot, invdyn.data(), eeName))
else:
res.effector_references[
eeName][:, k_t] = res.effector_references[
eeName][:, k_t - 1]
res.d_effector_references[eeName][:, k_t] = np.matrix(
np.zeros(6)).T
res.dd_effector_references[eeName][:, k_t] = np.matrix(
np.zeros(6)).T
# solve HQP for the current time
HQPData = invdyn.computeProblemData(t, q, v)
if cfg.WB_VERBOSE and t < phase.time_trajectory[0] + dt:
print "final data for phase ", pid
HQPData.print_all()
sol = solver.solve(HQPData)
dv = invdyn.getAccelerations(sol)
res = storeData(k_t, res, q, v, dv, invdyn, sol)
# update state
v_mean = v + 0.5 * dt * dv
v += dt * dv
q = pin.integrate(robot.model(), q, dt * v_mean)
if cfg.WB_VERBOSE == 2:
print "v = ", v
print "dv = ", dv
if cfg.WB_VERBOSE and int(t / dt) % cfg.IK_PRINT_N == 0:
printIntermediate(v, dv, invdyn, sol)
try:
checkDiverge(res, v, dv)
except ValueError:
return stopHere()
# end while t \in phase_t (loop for the current contact phase)
if swingPhase and cfg.EFF_CHECK_COLLISION:
phaseValid, t_invalid = validator.check_motion(
res.q_t[:, phase_interval[0]:k_t])
#if iter_for_phase < 3 :# FIXME : debug only, force limb-rrt
# phaseValid = False
if not phaseValid:
print "Phase " + str(pid) + " not valid at t = " + str(
t_invalid)
if t_invalid <= cfg.EFF_T_PREDEF or t_invalid >= (
(t_phase_end - t_phase_begin) - cfg.EFF_T_PREDEF):
print "Motion is invalid during predefined phases, cannot change this."
return stopHere()
if effectorCanRetry():
print "Try new end effector trajectory."
try:
for eeName, oldTraj in dic_effectors_trajs.iteritems(
):
if oldTraj: # update the traj in the map
placement_init = JointPlacementForEffector(
phase_prev, eeName)
placement_end = JointPlacementForEffector(
phase_next, eeName)
ref_traj = generateEndEffectorTraj(
time_interval, placement_init,
placement_end, iter_for_phase + 1,
res.q_t[:, phase_interval[0]:k_t],
phase_prev, phase, phase_next,
fullBody, eeName, viewer)
dic_effectors_trajs.update(
{eeName: ref_traj})
if viewer and cfg.DISPLAY_ALL_FEET_TRAJ:
display_tools.displaySE3Traj(
ref_traj, viewer.client.gui,
viewer.sceneName,
eeName + "_traj_" + str(pid), cfg.
Robot.dict_limb_color_traj[eeName],
time_interval,
cfg.Robot.dict_offset[eeName])
viewer.client.gui.setVisibility(
eeName + "_traj_" + str(pid),
'ALWAYS_ON_TOP')
except ValueError, e:
print "ERROR in generateEndEffectorTraj :"
print e.message
return stopHere()
else:
print "End effector method choosen do not allow retries, abort here."
return stopHere()
else: # no effector motions, phase always valid (or bypass the check)
phaseValid = True
if cfg.WB_VERBOSE:
print "Phase " + str(pid) + " valid."
if phaseValid:
# display all the effector trajectories for this phase
if viewer and cfg.DISPLAY_FEET_TRAJ and not cfg.DISPLAY_ALL_FEET_TRAJ:
for eeName, ref_traj in dic_effectors_trajs.iteritems():
if ref_traj:
display_tools.displaySE3Traj(
ref_traj, viewer.client.gui, viewer.sceneName,
eeName + "_traj_" + str(pid),
cfg.Robot.dict_limb_color_traj[eeName],
time_interval, cfg.Robot.dict_offset[eeName])
viewer.client.gui.setVisibility(
eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP')
if cfg.PLOT: # add current ref_traj to the list for plotting
stored_effectors_ref[eeName] += [ref_traj]
tol = 1e-5
import os
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
model = robot.model()
data = robot.data()
q = robot.model().neutralConfiguration
q[2] += 0.84
print "q:", q.transpose()
taskAM = tsid.TaskAMEquality("task-AM", robot)
Kp = 100 * np.matrix(np.ones(3)).transpose()
Kd = 20.0 * np.matrix(np.ones(3)).transpose()
taskAM.setKp(Kp)
taskAM.setKd(Kd)
assert np.linalg.norm(Kp - taskAM.Kp, 2) < tol
assert np.linalg.norm(Kd - taskAM.Kd, 2) < tol
am_ref = np.matrix(np.zeros(3)).transpose()
traj = tsid.TrajectoryEuclidianConstant("traj_se3", am_ref)
sample = tsid.TrajectorySample(0)
t = 0.0
dt = 0.001