def computeZMP(dm, ddq, H_plane_0, gravity_vector):
dq = get_alldq(dm)
Resultante0 = lgsm.zeros(3)
Moment0 = lgsm.zeros(3)
for i in range(dm.nbSegments()):
m, Inertia, p_0_scom, J_scom_0_0, dJ_scom_0_0__dq = _getSegmentCoMPropertiesIn0(dm, i)
V_scom_0_0 = J_scom_0_0 * dq
dV_scom_0_0 = J_scom_0_0 * ddq + dJ_scom_0_0__dq
linacc = dV_scom_0_0[3:6] - gravity_vector
rotacc = dV_scom_0_0[0:3]
rotvel = V_scom_0_0[0:3]
R0 = m * linacc
M0 = m * lgsm.crossprod(p_0_scom, linacc) + (Inertia * rotacc - lgsm.crossprod((Inertia * rotvel), rotvel) )
Resultante0 += R0
Moment0 += M0
n = gravity_vector/lgsm.norm(gravity_vector)
zmp = lgsm.crossprod(n, Moment0) / lgsm.dotprod(n, Resultante0)
zmp_XY = H_plane_0 * zmp
return np.array(zmp_XY[0:2]).flatten()
def _getSegmentCoMPropertiesIn0(dm, index):
H_0_s = dm.getSegmentPosition(index)
p_s_scom = dm.getSegmentCoM(index)
J_s_0_s = dm.getSegmentJacobian(index)
T_s_0_s = dm.getSegmentVelocity(index)
dJ_s_0_s__dq = dm.getSegmentJdotQdot(index)
#inertia info
m = dm.getSegmentMass(index)
Inertia = lgsm.np.diag( [v for v in dm.getSegmentMomentsOfInertia(index).flat] )
# CoM Position
p_0_scom = H_0_s * p_s_scom
# CoM Jacobian
H_s_scomNoRot = lgsm.Displacement()
H_s_scomNoRot.setTranslation(p_s_scom)
H_s_scomNoRot.setRotation(H_0_s.getRotation().inverse())
Ad_scomNoRot_s = H_s_scomNoRot.inverse().adjoint()
J_scomNotRot_0_0 = Ad_scomNoRot_s * J_s_0_s #scomNoRot has the same orientation as 0, so J_scomNotRot_0_scomNotRot = J_scomNotRot_0_0
J_scom_0_0 = J_scomNotRot_0_0 #[3:6,:]
# CoM dJacobian_dq
T_scomNoRot_0_s = lgsm.Twist(T_s_0_s)
T_scomNoRot_0_s.setLinearVelocity(lgsm.zeros(3))
dAd_scomNoRot_0_s = getDerivativeAdjoint(Ad_scomNoRot_s, T_scomNoRot_0_s)
dJ_scomNotRot_0_0__dq = Ad_scomNoRot_s * dJ_s_0_s__dq + dAd_scomNoRot_0_s * T_s_0_s
dJ_scom_0_0__dq = dJ_scomNotRot_0_0__dq #[3:6,:]
return m, Inertia, p_0_scom, J_scom_0_0, dJ_scom_0_0__dq
def get_alldq(dynModel):
dq = lgsm.zeros(dynModel.nbDofs())
if dynModel.hasFixedRoot():
dq[:] = dynModel.getJointVelocities().copy()
else:
dq[0:6] = dynModel.getFreeFlyerVelocity().copy()
dq[6: ] = dynModel.getJointVelocities().copy()
return dq
def __init__(self, name, time_step, model, rname, phy, icsync, solver, use_reduced_problem_no_ddq, jmap):
super(Deliverable1YController, self).__init__(rtt.PyTaskFactory.CreateTask(rname))
self.s.setPeriod(time_step)
self.t = 0
self.dt = time_step
self.rname = rname
self.ctrl = xic.ISIRController(model, rname, phy,icsync, solver, use_reduced_problem_no_ddq)
self.model = model
print "declaring observers"
#observers for joints pos and vel
self.observer_q =[]
self.observer_dq=[]
# we need some task to keep the robot upper body stable
pi = np.pi
rad2deg = 180.0/pi;
# initial joint configuration
init_l_shoulder_roll = pi/8.
init_r_shoulder_roll = pi/8.
init_l_elbow_pitch = pi/2.
init_r_elbow_pitch = pi/2.
Weight_back_task = 10.
Weight_head_task = 10.
Weight_arm_task = 10.
## back task: to keep the back stable
print "creation back task"
back_dofs = [jmap[rname+"."+n] for n in ['torso_pitch', 'torso_roll', 'torso_yaw']]
back_init = lgsm.zeros(3)
self.backTask = self.ctrl.createPartialTask("back", back_dofs, w=Weight_back_task, kp=16., q_des=back_init)
## head stabilization task:
print "creation head task"
head_init = self.model.getSegmentPosition(self.model.getSegmentIndex(rname+".head")).copy()
self.headTask = self.ctrl.createFrameTask("head_task", rname+'.head', lgsm.Displacement(), "RZ", w=Weight_head_task, kp=10., pose_des=head_init )
## arm task: to not move the arms
print "creation r_arm task"
r_arm_dofs =[jmap[rname+"."+n] for n in ['r_shoulder_roll','r_shoulder_yaw','r_shoulder_pitch','r_elbow_pitch', 'r_elbow_yaw','r_wrist_pitch']]
r_arm_init = lgsm.vector(init_r_shoulder_roll,0.,0.,init_r_elbow_pitch,0.,0.)
self.rArmTask= self.ctrl.createPartialTask("r_arm", r_arm_dofs, w=Weight_arm_task, kp=16., q_des=r_arm_init)
print "creation l_arm task"
l_arm_dofs =[jmap[rname+"."+n] for n in ['l_shoulder_roll','l_shoulder_yaw','l_shoulder_pitch','l_elbow_pitch', 'l_elbow_yaw','l_wrist_pitch']]
l_arm_init = lgsm.vector(init_l_shoulder_roll,0.,0.,init_l_elbow_pitch,0.,0.)
self.lArmTask= self.ctrl.createPartialTask("l_arm", l_arm_dofs, w=Weight_arm_task, kp=16., q_des=l_arm_init)
self.fi = 0.
def setJoint(self, id):
#id is the identificator of the slider/joint
#joint_signal_mapping.mapping(id) is the slider,
#Value is int and is in [-500,500] so we divide by 100.0
self.joint_position = self.robot.getJointPositions()
self.joint_position[id] = self.joint_signal_mapping.mapping(id).value()/100.0
self.robot.setJointPositions(self.joint_position)
self.robot.setJointVelocities(lgsm.zeros(self.dof))
joint_name = ""
for jname, jrank in self.joint_map.items():
if jrank == id:
joint_name = jname
self.label_signal_mapping.mapping(id).setText("[%.2f]" % self.joint_position[id])
def doUpdate(self, c):
model = self.model
if len(c.cpt) == 0:
print "no contact yet"
else:
# That what we get
print "Nb of contact: ", len(c.cpt)
print "for contact 0: "
cc = c.cpt[0]
print " ai :", cc.ai[:]
print " aj :", cc.aj[:]
print " gap:", cc.gap
print " ni :", cc.ni[:]
print " nj :", cc.nj[:]
print " normalForce:", cc.normalForce
time.sleep(.1)
tau = lgsm.zeros(model.nbInternalDofs())
self.tau_out.write(tau)
def __init__(self, robot, robot_name, joint_map):
super(JointGui, self).__init__()
self.robot = robot
self.dof = self.robot.getJointSpaceDim()
self.joint_position = lgsm.zeros(self.dof)
self.joint_gui_widget = QtGui.QWidget()
if joint_map:
self.joint_map = joint_map
else:
#joint_map is empty, use str(joint_rank) as dictionnary key
self.joint_map = {str(v):k for v, k in [[i,i] for i in range(self.dof)]}
#Sliders to joint map
self.joint_signal_mapping = QtCore.QSignalMapper(self)
self.label_signal_mapping = QtCore.QSignalMapper(self)
self.sliders = []
self.initGui(robot_name)
self.setWidget(self.joint_gui_widget)
self.setGeometry(300, 300, 450, 500)
self.show()
def doUpdate(self, c):
model = self.model
if len(c.cpt) == 0:
print "no contact yet"
else:
# That what we get
print "---------------"
print "Contact points:"
print "Nb of contact: ", len(c.cpt)
i_contact = 0
for cc in list(c.cpt):
print "for contact "+str(i_contact)+" : "
print " ai :", cc.ai[:]
print " aj :", cc.aj[:]
print " gap:", cc.gap
print " ni :", cc.ni[:]
print " nj :", cc.nj[:]
print " normalForce:", cc.normalForce
i_contact = i_contact+1
tau = lgsm.zeros(model.nbInternalDofs())
self.tau_out.write(tau)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
##### CTRL
import xde_isir_controller as xic
ctrl = xic.ISIRController(dynModel, rname, wm.phy, wm.icsync, "quadprog", True)
jointConst = ctrl.add_constraint( xic.JointLimitConstraint(ctrl.getModel(), -0.5 * lgsm.ones(N), 0.8 * lgsm.ones(N), .1) )
#jointConst.setJointLimits(-0.5 * lgsm.ones(N), 0.8 * lgsm.ones(N))
gposdes = 1.2 * lgsm.ones(N)
gveldes = lgsm.zeros(N)
fullTask = ctrl.createFullTask("full", "INTERNAL", w=1., kp=20.)
fullTask.set_q(gposdes)
fullTask.set_qdot(gveldes)
##### OBSERVERS
jpobs = ctrl.add_updater(xic.observers.JointPositionsObserver(ctrl.getModel()))
###### SIMULATE
ctrl.s.start()
wm.startAgents()
wm.phy.s.agent.triggerUpdate()
##### ROBOT
rname = "robot"
robotWorld = xrl.createWorldFromUrdfFile("resources/Robot3T.xml", rname, [0,0,0,1,0,0,0], True, 0.001, 0.001, use_collada_color=False)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(False)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
robot.setJointPositions(lgsm.vector(.5,.5,.5))
dynModel.setJointPositions(lgsm.vector(.5,.5,.5))
robot.setJointVelocities(lgsm.zeros(N))
dynModel.setJointVelocities(lgsm.zeros(N))
##### SET INTERACTION
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, 1.5)
#robot.enableContactWithRobot("sphere1", True)
#wm.contact.showContacts([(rname+"."+b,"sphere1.sphere") for b in ["link_x", "link_y", "link_z"]]) # to display contact
robot.enableAllContacts(True)
##### CTRL
import xde_isir_controller as xic
ctrl = xic.ISIRController(dynModel, rname, wm.phy, wm.icsync, "qld", True)
ctrl.controller.takeIntoAccountGravity(False)
#jointConst = ctrl.add_constraint( xic.JointLimitConstraint(ctrl.getModel(), .2) )
vhWorld = desc.simple.scene.createWorld()
vhName = "manikin"
human.addManikin(vhWorld, vhName, mass, size, H_initPos, .1)
#marker_coord, marker_body = scene.buildCodaCouplings(vhWorld, vhName, MARKER_FILE) # Configure couplings (must be done before adding manikin to world)
wm.addWorld(vhWorld)
vh = wm.phy.s.GVM.Robot(vhName)
wm.ms.assembleSystem()
vh.enableGravity(False)
N = vh.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(vhWorld)
# Set initial state
vh.setJointPositions(lgsm.zeros(N))
dynModel.setJointPositions(lgsm.zeros(N))
vh.setJointVelocities(lgsm.zeros(N))
dynModel.setJointVelocities(lgsm.zeros(N))
#vh.lockSegmentJoints2("Thoracic Spine 4")
#vh.lockSegmentJoints2("Head Center of Gravity")
#vh.lockSegmentJoints2("Right Clavicular")
#vh.lockSegmentJoints2("Left Clavicular")
#
#wm.phy.s.Connectors.OConnectorRobotState.new("ocpos", vhName+"_", vhName)
#wm.phy.s.Connectors.IConnectorPDCouplingList.new("icpdcoupl", "Coda_refs_"+vhName, POSITION_MODE) # can be used as "POSVEL_MODE" also, in this case you need to give the reference position and velocity of each PDCoupling
#######################
## Create DataPlayer ##
rname = "robot"
robotWorld = xrl.createWorldFromUrdfFile(xr.kuka, rname, [0,0,0,0,1,0,0], True, 10.0, 0.01, use_collada_color=False)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
##### CTRL
import xde_isir_controller as xic
ctrl = xic.ISIRController(dynModel, rname, wm.phy, wm.icsync, "quadprog", True)
gposdes = lgsm.zeros(N)
gveldes = lgsm.zeros(N)
fullTask = ctrl.createFullTask("full", w=0.001, kp=0, kd=10)
fullTask.set_q(gposdes)
fullTask.set_qdot(gveldes)
torqueTask = ctrl.createTorqueTask("torque", [1], w=1.)
torqueTask.set_tau(lgsm.vector([1.51]))
##### OBSERVERS
jpobs = ctrl.add_updater(xic.observers.JointPositionsObserver(ctrl.getModel()))
tpobs = ctrl.add_updater(xic.observers.TorqueObserver(ctrl))
##### SIMULATE
#create connectors to get robot k1g state 'k1g_q', 'k1g_qdot', 'k1g_Hroot', 'k1g_Troot', 'k1g_H'
wm.phy.s.Connectors.OConnectorRobotState.new("ocpos"+rname, rname+"_", rname)
wm.phy.s.Connectors.IConnectorRobotJointTorque.new("ict"+rname, rname+"_", rname)
#connector = wm.phy.s.Connectors.OConnectorContactBody.new("robotground", "port_robotground")
#connector.addInteraction("romeo.r_ankle", "ground.ground")
#wm.phy.getPort("port_robotground").connectTo(control.getPort("contacts"))
wm.phy.getPort(rname+"_q").connectTo(control.getPort("q"))
wm.phy.getPort(rname+"_qdot").connectTo(control.getPort("qdot"))
wm.phy.getPort(rname+"_Troot").connectTo(control.getPort("t"))
wm.phy.getPort(rname+"_Hroot").connectTo(control.getPort("d"))
control.getPort("tau").connectTo(wm.phy.getPort(rname+"_tau"))
# Configure the robot
qinit = lgsm.zeros(N)
for name, val in [("LShoulderPitch", pi/2.), ("RShoulderPitch", pi/2.), ("LElbowRoll", -pi/2.), ("RElbowRoll", pi/2.)]:
qinit[jointmap[rname+"."+name]] = val
##### SET INTERACTION
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, mu_sys)
romeo.enableContactWithBody("ground.ground", True)
wm.contact.showContacts([(rname+"."+b,"ground.ground") for b in ["l_ankle", "r_ankle"]])
##### SET INITIAL CONFIGURATION
romeo.setJointPositions(qinit)
dynmodel.setJointPositions(qinit)
romeo.setJointVelocities(lgsm.zeros(N))
dynmodel.setJointVelocities(lgsm.zeros(N))
#control.s.setPeriod(TIME_STEP)
def __init__(self, name, time_step, model, rname, phy, icsync, solver, use_reduced_problem_no_ddq, jmap, observer):
super(VHController, self).__init__(rtt.PyTaskFactory.CreateTask(rname))
self.s.setPeriod(time_step)
self.t = 0
self.dt = time_step
self.rname = rname
self.ctrl = xic.ISIRController(model, rname, phy,icsync, solver, use_reduced_problem_no_ddq)
self.model = model
###################### PARAMETERS ##############################
pi = np.pi
rad2deg = 180.0/pi;
# height of the waist
waist_z = 0.58
self.n = self.model.nbInternalDofs()
head_init = self.model.getSegmentPosition(self.model.getSegmentIndex(rname+".head")).copy()
com_init = self.model.getCoMPosition().copy()
self.com_init = com_init
rhand_init = self.model.getSegmentPosition(self.model.getSegmentIndex(rname+".r_hand")).copy()
lhand_init = self.model.getSegmentPosition(self.model.getSegmentIndex(rname+".l_hand")).copy()
qinit = lgsm.zeros(self.n)
# correspond to: l_elbow_pitch r_elbow_pitch l_knee r_knee l_ankle_pitch r_ankle_pitch l_shoulder_roll r_shoulder_roll
for name, val in [("l_elbow_pitch", np.pi/2.), ("r_elbow_pitch", np.pi/2.), ("l_knee", -0.05), ("r_knee", -0.05), ("l_ankle_pitch", -0.05), ("r_ankle_pitch", -0.05), ("l_shoulder_roll", np.pi/2.), ("r_shoulder_roll", np.pi/2.)]:
qinit[jmap[rname+"."+name]] = val
#### WEIGHTS OF THE TASKS
Weight_full_task = 0.001
Weight_back_task = 10.
Weight_waist_task = 1.
Weight_head_task = 10.
Weight_COM_task = 1000.
Weight_hand_task = 100.
Weight_hand_task_or = 10.
Weight_force_task = 100.
# Weight_full_task = 0.0001
# Weight_back_task = 0.001
# Weight_waist_task = 0.4
# Weight_head_task = 0.5
# Weight_COM_task = 10.
# Weight_hand_task = 5.
# Weight_hand_task_or = 4.
# Weight_force_task = 1.
# the desired force is expressed in the world frame
force_desired = lgsm.vector(0,0,0,0,0,0) # mux, muy, muz, fx, fy,fz
#### TASKS
## full task: bring joints to the initial position
#default: Kd= 2*sqrt(Kp)
self.fullTask = self.ctrl.createFullTask("full", "INTERNAL", w=Weight_full_task, kp=0., KD = 9., q_des=qinit)
## waist task: better balance
self.waistTask = self.ctrl.createFrameTask("waist", rname+'.waist', lgsm.Displacement(), "R", w=Weight_waist_task, kp=10., pose_des=lgsm.Displacement(0,0,.58,1,0,0,0))
## back task: to keep the back stable
back_dofs = [jmap[rname+"."+n] for n in ['torso_pitch', 'torso_roll', 'torso_yaw']]
self.backTask = self.ctrl.createPartialTask("back", back_dofs, w=Weight_back_task, kp=16., q_des=lgsm.zeros(3))
## head stabilization task:
self.headTask = self.ctrl.createFrameTask("head_task", rname+'.head', lgsm.Displacement(), "RZ", w=Weight_head_task, kp=10., pose_des=head_init )
## COM task
self.CoMTask = self.ctrl.createCoMTask("com", "XY", w=Weight_COM_task, kp=50., pose_des=lgsm.Displacement(t=com_init))
## contact tasks for the feet
sqrt2on2 = lgsm.np.sqrt(2.)/2.
RotLZdown = lgsm.Quaternion(-sqrt2on2,0.0,-sqrt2on2,0.0) * lgsm.Quaternion(0.0,1.0,0.0,0.0)
RotRZdown = lgsm.Quaternion(0.0, sqrt2on2,0.0, sqrt2on2) * lgsm.Quaternion(0.0,1.0,0.0,0.0)
self.ctrl.createContactTask("CLF0", rname+".l_foot", lgsm.Displacement([-.039,-.027,-.031]+ RotLZdown.tolist()), 1., 0.) # mu, margin
self.ctrl.createContactTask("CLF1", rname+".l_foot", lgsm.Displacement([-.039, .027,-.031]+ RotLZdown.tolist()), 1., 0.) # mu, margin
self.ctrl.createContactTask("CLF2", rname+".l_foot", lgsm.Displacement([-.039, .027, .099]+ RotLZdown.tolist()), 1., 0.) # mu, margin
self.ctrl.createContactTask("CLF3", rname+".l_foot", lgsm.Displacement([-.039,-.027, .099]+ RotLZdown.tolist()), 1., 0.) # mu, margin
self.ctrl.createContactTask("CRF0", rname+".r_foot", lgsm.Displacement([-.039,-.027, .031]+ RotRZdown.tolist()), 1., 0.) # mu, margin
self.ctrl.createContactTask("CRF1", rname+".r_foot", lgsm.Displacement([-.039, .027, .031]+ RotRZdown.tolist()), 1., 0.) # mu, margin
self.ctrl.createContactTask("CRF2", rname+".r_foot", lgsm.Displacement([-.039, .027,-.099]+ RotRZdown.tolist()), 1., 0.) # mu, margin
self.ctrl.createContactTask("CRF3", rname+".r_foot", lgsm.Displacement([-.039,-.027,-.099]+ RotRZdown.tolist()), 1., 0.) # mu, margin
## frame tasks for putting the hand in contact with the table
#this is the controlled frame - attached to the hand
H_hand_tool = lgsm.Displacement(0., 0., 0, 1, 0, 0, 0)
#Task_hand_controlled_var = "RXYZ" "RXYZ" "R" "XY" "Z" ..
self.RHand_task = self.ctrl.createFrameTask("rhand", rname+'.r_hand', H_hand_tool, "XYZ", w=Weight_hand_task, kp=36., pose_des=rhand_init)
self.LHand_task = self.ctrl.createFrameTask("lhand", rname+'.l_hand', H_hand_tool, "XYZ", w=Weight_hand_task, kp=36., pose_des=lhand_init)
self.orient_RHand_task = self.ctrl.createFrameTask("orient_rhand", rname+'.r_hand', H_hand_tool, "R", w=Weight_hand_task_or, kp=36., pose_des=rhand_init)
self.orient_LHand_task = self.ctrl.createFrameTask("orient_lhand", rname+'.l_hand', H_hand_tool, "R",w=Weight_hand_task_or, kp=36., pose_des=lhand_init)
## force task for touching the table
self.force_RHand_Task = self.ctrl.createForceTask("force_rhand", rname+".r_hand", H_hand_tool, w=Weight_force_task, kp=16)
self.force_LHand_Task = self.ctrl.createForceTask("force_lhand", rname+".l_hand", H_hand_tool, w=Weight_force_task, kp=16)
# update the task goal
self.force_RHand_Task.setPosition(lgsm.Displacement()) # expressed in world frame
self.force_LHand_Task.setPosition(lgsm.Displacement()) # expressed in world frame
self.force_RHand_Task.setWrench(force_desired)
self.force_LHand_Task.setWrench(force_desired)
self.fsm = controlfsm.build(self, observer)
self.fi = 0.
# visualize contacts between the whole body and environment (ground + other items)
if contact_show_body_environment == True:
for e in environmentItems:
for b in robot.getSegmentNames():
print "Enabling visualization of contacts between "+b+" and "+e
wm.contact.showContacts([(robot_name+"."+b,e)])
# visualize contacts between the hands and environment (ground + other items)
if contact_show_hands_environment == True:
for e in environmentItems:
for b in ["l_hand", "r_hand"]:
print "Enabling visualization of contacts between "+b+" and "+e
wm.contact.showContacts([(robot_name+"."+b,e)])
##### SET INITIAL STATE OF THE ROBOT
qinit = lgsm.zeros(N)
# correspond to: l_elbow_pitch r_elbow_pitch l_knee r_knee l_ankle_pitch r_ankle_pitch l_shoulder_roll r_shoulder_roll
for name, val in [("l_elbow_pitch", pi/2.),
("r_elbow_pitch", pi/2.),
("l_knee", -0.05),
("r_knee", -0.05),
("l_ankle_pitch", -0.05),
("r_ankle_pitch", -0.05),
("l_shoulder_roll", pi/2.),
("r_shoulder_roll", pi/2.)]:
qinit[jmap[robot_name+"."+name]] = val
robot.setJointPositions(qinit)
dynModel.setJointPositions(qinit)
robot.setJointVelocities(lgsm.zeros(N))
dynModel.setJointVelocities(lgsm.zeros(N))
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
jmap = xrl.getJointMapping(xr.icub_simple, robot)
##### SET INTERACTION
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, 1.5)
robot.enableContactWithBody("ground.ground", True)
wm.contact.showContacts([(rname+"."+b,"ground.ground") for b in ["l_foot", "r_foot"]]) # to display contact
##### SET INITIAL STATE
qinit = lgsm.zeros(N)
for name, val in [("l_elbow_pitch", pi/8.), ("r_elbow_pitch", pi/8.), ("l_knee", -0.05), ("r_knee", -0.05), ("l_ankle_pitch", -0.05), ("r_ankle_pitch", -0.05), ("l_shoulder_roll", pi/8.), ("r_shoulder_roll", pi/8.)]:
qinit[jmap[rname+"."+name]] = val
robot.setJointPositions(qinit)
dynModel.setJointPositions(qinit)
robot.setJointVelocities(lgsm.zeros(N))
dynModel.setJointVelocities(lgsm.zeros(N))
##### CTRL
import xde_isir_controller as xic
ctrl = xic.ISIRController(dynModel, rname, wm.phy, wm.icsync, "quadprog", True)
##### SET TASKS
robot.enableGravity(True)
#robot.enableGravity(False)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
jmap = xrl.getJointMapping(xr.romeo_collision, robot)
##### SET INTERACTION
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, mu_sys)
robot.enableContactWithBody("ground.ground", True)
#wm.contact.showContacts([(rname+"."+b,"ground.ground") for b in ["l_ankle", "r_ankle"]]) # to display contact
##### SET INITIAL STATE
qinit = lgsm.zeros(N)
for name, val in [("LShoulderPitch", pi/2.), ("RShoulderPitch", pi/2.)]:
#for name, val in [("LShoulderPitch", pi/2.), ("RShoulderPitch", pi/2.), ("LHipPitch", -pi/15.), ("RHipPitch", -pi/15.), ("LKneePitch", 2*pi/15.), ("RKneePitch", 2*pi/15.), ("RAnklePitch", -pi/15.), ("LAnklePitch", -pi/15.)]:
qinit[jmap[rname+"."+name]] = val
#for name, val in [("LShoulderPitch", pi/2.), ("RShoulderPitch", pi/2.)]:
# qinit[jmap[rname+"."+name]] = val
robot.setJointPositions(qinit)
dynModel.setJointPositions(qinit)
robot.setJointVelocities(lgsm.zeros(N))
dynModel.setJointVelocities(lgsm.zeros(N))
##### CONTROLLER
import xde_isir_controller as xic
ctrl = xic.ISIRController(dynModel, rname, wm.phy, wm.icsync, "quadprog", False)
