...
 
Commits (14)
......@@ -231,7 +231,7 @@ def isNewPhase(ds1, ds2):
return False
def CSfromMomentumopt(planner_setting, cs, init_state, dyn_states, t_init = 0, connect_goal = True):
def CSfromMomentumopt(planner_setting, cs, init_state, dyn_states, t_init = 0., connect_goal = True):
"""
Create a ContactSequence and fill it with the results from momentumopt
:param planner_setting:
......@@ -276,7 +276,7 @@ def CSfromMomentumopt(planner_setting, cs, init_state, dyn_states, t_init = 0, c
L_t = append(L_t, L.reshape(3,1), axis = 1)
dL_t = append(dL_t, dL.reshape(3,1), axis = 1)
current_t += ds.dt
times = append(times, current_t)
times = append(times, round(current_t, 6))
if k > 0 and isNewPhase(dyn_states[k-1], ds) and p_id < cs_com.size() - 1:
#last k of current phase, first k of next one (same trajectories and time)
......
from hpp.corbaserver.rbprm.anymal import Robot
try:
from anymal_rbprm.anymal import Robot
except ImportError:
message = "ERROR: Cannot import anymal-rbprm package.\n"
message += "Did you correctly installed it?\n"
message +="https://github.com/humanoid-path-planner/anymal-rbprm"
raise ImportError(message)
MASS = 30.47 # cannot retrieve it from urdf because the file is not parsed here ...
## weight and gains used by TSID
......@@ -9,7 +15,6 @@ w_com = 1.0 # weight of center of mass task
w_am = 0.
w_posture = 0.01 # weight of joint posture task
w_rootOrientation = 1. # weight of the root's orientation task
w_forceRef = 1e-3 # weight of force regularization task
w_eff = 1.0 # weight of the effector motion task
kp_contact = 50.0 # proportional gain of contact constraint
kp_com = 30. # proportional gain of center of mass task
......@@ -23,6 +28,12 @@ level_posture = 1
level_rootOrientation = 1
level_am = 1
# The weight of the force regularization task of each contact will start at w_forceRef_init when creating a new contact,
# and then linearly reduce to w_forceRef_end over a time period of phase_duration * w_forceRef_time_ratio
w_forceRef_init = 1.
w_forceRef_end = 1e-5
w_forceRef_time_ratio = 0.5
YAW_ROT_GAIN = 1.
IK_eff_size = Robot.dict_size.copy()
......
from hpp.corbaserver.rbprm.hrp2 import Robot
## following info are already accessible once the urdf is loaded, but here we do not load the urdf ...
try:
from hrp2_rbprm.hrp2 import Robot
except ImportError:
message = "ERROR: Cannot import hrp2-rbprm package.\n"
message += "Did you correctly installed it?\n"
message +="https://gepgitlab.laas.fr/hrp2-dev/hrp2-rbprm"
raise ImportError(message)## following info are already accessible once the urdf is loaded, but here we do not load the urdf ...
MASS = 55.88363633
## weight and gains used by TSID
......
from hpp.corbaserver.rbprm.hyq_contact6D import Robot
try:
from hyq_rbprm.hyq import Robot
except ImportError:
message = "ERROR: Cannot import hyq-rbprm package.\n"
message += "Did you correctly installed it?\n"
message +="https://github.com/humanoid-path-planner/hyq-rbprm"
raise ImportError(message)
MASS = 76.07
## weight and gains used by TSID
......
from talos_rbprm.talos import Robot
try:
from talos_rbprm.talos import Robot
except ImportError:
message = "ERROR: Cannot import anymal-talos_rbprm package.\n"
message += "Did you correctly installed it?\n"
message +="https://github.com/humanoid-path-planner/talos-rbprm"
raise ImportError(message)
MASS = 90.27
GUIDE_STEP_SIZE = 1.
......@@ -25,7 +32,6 @@ w_am = 2e-2 # weight used for the minimization of the Angular momentum
w_am_track = 2e-2 # weight used for the tracking of the Angular momentum
w_posture = 0.1 # weight of joint posture task
w_rootOrientation = 1. # weight of the root's orientation task
w_forceRef = 1e-3 # weight of force regularization task
w_eff = 1.0 # weight of the effector motion task
kp_contact = 30.0 # proportional gain of contact constraint
kp_com = 20. # proportional gain of center of mass task
......@@ -39,6 +45,11 @@ level_com = 0
level_posture = 1
level_rootOrientation = 1
level_am = 1
# The weight of the force regularization task of each contact will start at w_forceRef_init when creating a new contact,
# and then linearly reduce to w_forceRef_end over a time period of phase_duration * w_forceRef_time_ratio
w_forceRef_init = 0.5
w_forceRef_end = 1e-5
w_forceRef_time_ratio = 0.5
#IK_dt = 0.001
IK_eff_size = Robot.dict_size.copy()
......
......@@ -9,7 +9,7 @@ DURATION_INIT = 3. # Time to init the motion
DURATION_FINAL = 3. # Time to stop the robot
DURATION_FINAL_SS = 1.
DURATION_SS = 2.
DURATION_DS = 2.
DURATION_DS = 4.
DURATION_TS = 0.4
DURATION_CONNECT_GOAL = 1.
......
......@@ -114,7 +114,7 @@ class LocoPlanner:
raise RuntimeError(
"The current contact sequence cannot be given as input to the end effector method selected.")
self.cs_ref = generate_effector_trajectories(self.cfg, self.cs_com, self.fullBody)
EffectorOutputs.assertRequirements(self.cs_ref)
EffectorOutputs.assertRequirements(self.cs_ref, self.cfg.Robot.cType == "_6_DOF")
self.cs_ref_iters += [self.cs_ref]
if self.cfg.DISPLAY_ALL_FEET_TRAJ:
......
......@@ -542,6 +542,19 @@ def setPreviousFinalValues(phase_prev, phase, cfg):
if cfg.IK_store_zmp:
phase_prev.zmp_t.append(phase.zmp_t(t), t)
phase_prev.wrench_t.append(phase.wrench_t(t), t)
if cfg.IK_store_contact_forces:
for eeName in phase_prev.effectorsInContact():
if phase.isEffectorInContact(eeName):
contact_forces = phase.contactForce(eeName)(t)
contact_normal_force = phase.contactNormalForce(eeName)(t)
else:
contact_normal_force = np.zeros(1)
if cfg.Robot.cType == "_3_DOF":
contact_forces = np.zeros(3)
else:
contact_forces = np.zeros(12)
phase_prev.contactForce(eeName).append(contact_forces, t)
phase_prev.contactNormalForce(eeName).append(contact_normal_force.reshape(1), t)
......
......@@ -186,8 +186,8 @@ class Requirements():
return True
@classmethod
def requireEffectorTrajectories(cls, cs):
if not cs.haveEffectorsTrajectories(1e-2):
def requireEffectorTrajectories(cls, cs, c_type):
if not cs.haveEffectorsTrajectories(1e-2, c_type == "_6_DOF"):
print("- Contact sequence do not have consistent effector trajectories.")
return False
return True
......@@ -239,7 +239,7 @@ class Requirements():
print("- contact forces trajectories for each effector in contact")
@classmethod
def assertRequirements(cls, cs):
def assertRequirements(cls, cs, use_effector_rotation = False):
#print("# Assert requirements : ")
if cls.timings:
assert cs.haveTimings(), "Contact sequence do not have consistent timings."
......@@ -274,7 +274,8 @@ class Requirements():
if cls.torqueTrajectories:
assert cs.haveTorquesTrajectories(), "Contact sequence do not have consistent torques trajectories"
if cls.effectorTrajectories:
assert cs.haveEffectorsTrajectories(1e-2), "Contact sequence do not have consistent effector trajectories."
assert cs.haveEffectorsTrajectories(1e-2, use_effector_rotation), \
"Contact sequence do not have consistent effector trajectories."
if cls.contactForcesTrajectories:
assert cs.haveContactForcesTrajectories(), "Contact sequence do not have consistent contact forces trajectories."
#print("# Assert requirements done.")
......@@ -294,7 +295,8 @@ class Requirements():
if cfg.IK_store_centroidal:
assert cs.haveCentroidalTrajectories (), "Contact sequence do not have consistent centroidal trajectories."
if cfg.IK_store_effector:
assert cs.haveEffectorsTrajectories(1e-2), "Contact sequence do not have consistent effector trajectories."
assert cs.haveEffectorsTrajectories(1e-2, cfg.Robot.cType == "_6_DOF"), \
"Contact sequence do not have consistent effector trajectories."
if cfg.IK_store_contact_forces:
assert cs.haveContactForcesTrajectories(), "Contact sequence do not have consistent contact forces trajectories."
if cfg.IK_store_zmp:
......@@ -361,7 +363,7 @@ class Requirements():
if not cls.requireTorqueTrajectories(cs):
return False
if cls.effectorTrajectories:
if not cls.requireEffectorTrajectories(cs):
if not cls.requireEffectorTrajectories(cs, cfg.Robot.cType):
return False
if cls.contactForcesTrajectories:
if not cls.requireContactForcesTrajectories(cs):
......
......@@ -409,7 +409,7 @@ def discretizeCurve(curve,dt):
numPoints = round((curve.max() - curve.min()) / dt ) + 1
res = np.zeros([curve.dim(), numPoints])
timeline = np.zeros(numPoints)
t = curve.min()
t = curve.min() + 0.0001 # add an epsilon to be sure to be AFTER the discontinuities at each phase changes
for i in range(numPoints):
res[:,i] = curve(t)
timeline[i] = t
......
......@@ -72,13 +72,14 @@ def getCurrentEffectorAcceleration(robot, data, eeName):
return robot.frameAcceleration(data, id)
def createContactForEffector(cfg, invdyn, robot, eeName, patch):
def createContactForEffector(cfg, invdyn, robot, eeName, patch, use_force_reg = True):
"""
Add a contact task in invdyn for the given effector, at it's current placement
:param invdyn:
:param robot:
:param eeName: name of the effector
:param patch: the ContactPatch object to use. Take friction coefficient and placement for the contact from this object
:param use_force_reg: if True, use the cfg.w_forceRef_init otherwise use cfg.w_forceRef_end
:return: the contact task
"""
contactNormal = np.array(cfg.Robot.dict_normal[eeName])
......@@ -89,6 +90,7 @@ def createContactForEffector(cfg, invdyn, robot, eeName, patch):
if patch.contact_model.contact_type == ContactType.CONTACT_POINT:
contact = tsid.ContactPoint("contact_" + eeName, robot, eeName, contactNormal, patch.friction, cfg.fMin, cfg.fMax)
mask = np.ones(3)
force_ref = np.zeros(3)
contact.useLocalFrame(False)
logger.info("create contact point")
elif patch.contact_model.contact_type == ContactType.CONTACT_PLANAR:
......@@ -96,6 +98,7 @@ def createContactForEffector(cfg, invdyn, robot, eeName, patch):
contact = tsid.Contact6d("contact_" + eeName, robot, eeName, contact_points, contactNormal, patch.friction, cfg.fMin,
cfg.fMax)
mask = np.ones(6)
force_ref = np.zeros(6)
logger.info("create rectangular contact")
logger.info("contact points : \n %s", contact_points)
else:
......@@ -103,7 +106,12 @@ def createContactForEffector(cfg, invdyn, robot, eeName, patch):
contact.setKp(cfg.kp_contact * mask)
contact.setKd(2.0 * np.sqrt(cfg.kp_contact) * mask)
contact.setReference(patch.placement)
invdyn.addRigidContact(contact, cfg.w_forceRef)
contact.setForceReference(force_ref)
if use_force_reg:
w_forceRef = cfg.w_forceRef_init
else:
w_forceRef = cfg.w_forceRef_end
invdyn.addRigidContact(contact, w_forceRef)
return contact
......@@ -257,39 +265,23 @@ def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None):
if t > phase_prev.effectorTrajectory(eeName).max():
placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
phase_prev.effectorTrajectory(eeName).append(placement, t)
if first_iter_for_phase:
for eeName in phase.effectorsWithTrajectory():
placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
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:
for eeName in phase.effectorsWithTrajectory():
placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
else:
phase.effectorTrajectory(eeName).append(placement, t)
def appendContactForcesTrajs(first_iter_for_phase = False):
if first_iter_for_phase and phase_prev:
for eeName in phase_prev.effectorsInContact():
if t > phase_prev.contactForce(eeName).max():
if phase.isEffectorInContact(eeName):
contact = dic_contacts[eeName]
contact_forces = invdyn.getContactForce(contact.name, sol)
contact_normal_force = np.array(contact.getNormalForce(contact_forces))
else:
contact_normal_force = np.zeros(1)
if cfg.Robot.cType == "_3_DOF":
contact_forces = np.zeros(3)
else:
contact_forces = np.zeros(12)
phase_prev.contactForce(eeName).append(contact_forces, t)
phase_prev.contactNormalForce(eeName).append(contact_normal_force.reshape(1), t)
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)
......@@ -411,7 +403,7 @@ def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None):
getCurrentEffectorPosition(robot, invdyn.data(), eeName),
cfg.Robot.cType == "_6_DOF")
# create the contacts :
contact = createContactForEffector(cfg, invdyn, robot, eeName, phase0.contactPatch(eeName))
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
......@@ -514,6 +506,7 @@ def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None):
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
......@@ -526,6 +519,7 @@ def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None):
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)
......@@ -539,6 +533,15 @@ def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None):
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')
......@@ -591,6 +594,16 @@ def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None):
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())
......