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.
Esempio n. 5
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])
Esempio n. 6
0
    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)
Esempio n. 7
0
    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()
Esempio n. 8
0
    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
Esempio n. 13
0
#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)