def lookAt(_from, _to, _up): # vec aligned with -z # up aligned with y vec = _to - _from nvec = lgsm.normalize(vec) minusZ = lgsm.vector(0,0,-1) Q_0_Z = rotationToAlignFirstToSecond(minusZ, vec) Up_in_Z = Q_0_Z.inverse() * _up ov_Y = lgsm.vector(1,0,0) # = getOrthogonalityData(minusZ, Y) ov_UP, t, st, ct = getOrthogonalityData(minusZ, Up_in_Z) if ov_UP is not None: ov_diff, t_diff, st_diff, ct_diff = getOrthogonalityData(ov_Y, ov_UP) if ov_diff is not None: Q_Z_UP = lgsm.Quaternion.fromAxisAngle( ov_diff, t_diff) else: if abs(t_diff) <1e-6: Q_Z_UP = lgsm.Quaternion() else: Q_Z_UP = lgsm.Quaternion(0,0,0,1) else: Q_Z_UP = lgsm.Quaternion() H = lgsm.Displacement() H.setTranslation(_from) H.setRotation(Q_0_Z * Q_Z_UP) return H
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 doUpdate(self, dt): if self.controller.t-self.t_ini <= self.t_duration: force_desired = lgsm.vector(0,0,0,0,0,-fmax*(self.controller.t-self.t_ini)/self.t_duration) self.controller.force_RHand_Task.setWrench(force_desired) p_com = compute_CoM_pos_forward(fd=-fmax+self.controller.fi, fi=self.fi, f=self.observer.getPort("tau").read()[0][0,0], delta = delta_com_forward) p_com = self.controller.com_init+p_com self.controller.CoMTask.setPosition(lgsm.Displacement(t=p_com))
def updateHook(self): sm_vel, sm_vel_ok = self.sm_in_port.read() #Hack self.camera = self.camera_interface.getCameraDisplacement("mainViewportBaseCamera") H_0_c = self.camera H_0_b = self.cursor.getPosition() H_b_0 = H_0_b.inverse() H_b_c = H_b_0 * H_0_c H_b_c.setTranslation(lgsm.vector([0,0,0])) if self.pdc_enabled: if "sm_pdc" in self.phy.s.getComponents(): self.pdc.setDesiredPosition(H_0_b, H_0_b) if sm_vel_ok : sm_vel = H_b_c.adjoint() * sm_vel self.cursor.setVelocity(sm_vel) else: self.cursor.setVelocity(lgsm.Twist()) self.pos_out.write(self.cursor.getPosition())
def updateHook(self): sm_vel, sm_vel_ok = self.sm_in_port.read() #Hack self.camera = self.camera_interface.getCameraDisplacement( "mainViewportBaseCamera") H_0_c = self.camera H_0_b = self.cursor.getPosition() H_b_0 = H_0_b.inverse() H_b_c = H_b_0 * H_0_c H_b_c.setTranslation(lgsm.vector([0, 0, 0])) if self.pdc_enabled: if "sm_pdc" in self.phy.s.getComponents(): self.pdc.setDesiredPosition(H_0_b, H_0_b) if sm_vel_ok: sm_vel = H_b_c.adjoint() * sm_vel self.cursor.setVelocity(100 * sm_vel) #self.cursor.setVelocity(lgsm.Displacementd(-0.4, 0.4, 0.2,-0.0157,0,-0.9998,0)) #self.cursor.setVelocity(lgsm.Twistd([1,0,0, 0, 0, 0])) else: self.cursor.setVelocity(lgsm.Twist()) self.pos_out.write(self.cursor.getPosition())
def doUpdate(self, dt): if self.controller.t-self.t_ini <= self.t_duration: force_desired = lgsm.vector(0,0,0,0,0,-fmax*(1.-(self.controller.t-self.t_ini)/self.t_duration)) self.controller.force_RHand_Task.setWrench(force_desired) p_com = compute_CoM_pos_backward(fd=self.controller.fi, fi=self.fi, f=self.observer.getPort("tau").read()[0][0,0], delta = 0.*delta_com_forward) p_com = self.controller.com_init+p_com self.controller.CoMTask.setPosition(lgsm.Displacement(t=p_com)) else: self.controller.RHand_task.setPosition(lgsm.Displacement([table_contact_x,table_contact_y_R,table_contact_z+0.05]+o_right_hand.tolist()))
def doUpdate(self, q, qdot): model = self.model model.setJointPositions(q) model.setJointVelocities(qdot) # define output vectors q_des_out = q qdot_des_out = lgsm.vector([.1] * (model.nbInternalDofs())) kp_des_out = lgsm.vector([self.kp_des] * model.nbInternalDofs()) kd_des_out = lgsm.vector([self.kd_des] * model.nbInternalDofs()) # send output vectors self.kp_des_out.write(kp_des_out) self.kd_des_out.write(kd_des_out) self.q_des_out.write(q_des_out) self.qdot_des_out.write(qdot_des_out)
def get_infinity_traj(start, direction, R=.5, eps=360, go_left=True): """ """ Xvec = lgsm.vector(1,0) sinT = lgsm.crossprod(Xvec, direction)[0,0] cosT = lgsm.dotprod(Xvec , direction) start_angle = lgsm.np.arctan2(sinT, cosT) ortho_direction = lgsm.vector(-direction[1,0], direction[0,0]) left_center = start + ortho_direction * R right_center = start - ortho_direction * R traj = [] if go_left: for T in np.linspace(0, 2.*pi, eps): a = start_angle + T x = left_center[0,0] +R*np.cos(a - pi/2.) y = left_center[1,0] +R*np.sin(a - pi/2.) traj.append([x,y,a]) for T in np.linspace(0, 2.*pi, eps): a = start_angle - T x = right_center[0,0] +R*np.cos(a + pi/2.) y = right_center[1,0] +R*np.sin(a + pi/2.) traj.append([x,y,a]) else: for T in np.linspace(0, 2.*pi, eps): a = start_angle - T x = right_center[0,0] +R*np.cos(a + pi/2.) y = right_center[1,0] +R*np.sin(a + pi/2.) traj.append([x,y,a]) for T in np.linspace(0, 2.*pi, eps): a = start_angle + T x = left_center[0,0] +R*np.cos(a - pi/2.) y = left_center[1,0] +R*np.sin(a - pi/2.) traj.append([x,y,a]) return np.array(traj)
def disconnectRobot(self, phy, robot_name): robot = phy.s.GVM.Robot(robot_name) ndof = robot.getJointSpaceDim() tau = lgsm.vector([0] * ndof) self.tau_port.write(tau) robot.setJointVelocities(np.array([0.0]*ndof).reshape(ndof,1)) phy.s.Connectors.delete("ict"+robot_name) phy.s.Connectors.delete("ocpos"+robot_name)
def __init__(self, dynModel, H_0_plane, dt, gravity, up=None): Recorder.__init__(self) self.dynModel = dynModel self.dt = dt self.gravity = gravity if up is None: up = lgsm.vector([0,0,1]) self.gravity_vector = - self.gravity * up self.H_plane_0 = H_0_plane.inverse() self.prev_dq = get_alldq(self.dynModel)
def updateHook(self): sm_vel, sm_vel_ok = self.sm_in_port.read() #Hack self.camera = self.camera_interface.getCameraDisplacement( "mainViewportBaseCamera") H_0_c = self.camera H_0_b = self.cursor.getPosition() H_b_0 = H_0_b.inverse() H_b_c = H_b_0 * H_0_c H_b_c.setTranslation(lgsm.vector([0, 0, 0])) H_0 = lgsm.Displacementd(0.0, 0.0, 0.0, 1, 0, 0, 0) sm_vel = H_0.adjoint() * lgsm.Twist([0, 0, 0, 0.0, self.v_y, 0.0]) #self.cursor.setVelocity(100 * sm_vel) #if self.cursor.getPosition.Translation.x() <= 0.9 : #self.mov_dir = "aller" #if self.cursor.gettPosition.x() >= 1.9 : #self.mov_dir = "retour" #if self.mov_dir == "aller" : #self.v_y = self.v_y #if self.mov_dir == "retour" : #self.v_y = -self.v_y #self.count = self.count + 1 #if self.count % 50 == 0 : #self.v_y = -self.v_y #self.cursor.setPosition(lgsm.Displacementd(self.X_x, 0.0, 0.0, 0.7071067811865476, 0, 0, 0.7071067811865476)) self.cursor.setPosition( lgsm.Displacementd(5.65, 0.0, 0.0, 0.7071067811865476, 0, 0, 0.7071067811865476)) if self.X_x <= 0.5: self.mov_dir = "aller" if self.X_x >= 1.6: self.mov_dir = "retour" if self.mov_dir == "aller": self.X_x = self.X_x + 0.001 if self.mov_dir == "retour": self.X_x = self.X_x - 0.001
def addGround(world): current_path = os.path.dirname(os.path.abspath(inspect.getfile( inspect.currentframe()))) ground_world = desc.simple.scene.parseColladaFile(current_path+"/ground.dae") phy_ground_world = desc.simple.scene.parseColladaFile(current_path+"/ground_phy_50mm.dae", append_label_library=".phyground") desc.simple.graphic.addGraphicalTree(world, ground_world, node_name="ground") desc.simple.collision.addCompositeMesh(world, phy_ground_world, composite_name="ground.comp", offset=0.05, clean_meshes=False, ignore_library_conflicts=False) desc.simple.physic.addRigidBody(world, "ground", mass=1, contact_material="material.concrete") ground_position = lgsm.Displacementd() ground_position.setTranslation(lgsm.vector(0,0,-0.1)) desc.simple.physic.addFixedJoint(world, "ground.joint", "ground", ground_position) #Binding graph, phy and coll object ground_graph_node = desc.core.findInTree(world.scene.graphical_scene.root_node, "ground") ground_phy_node = desc.physic.findInPhysicalScene(world.scene.physical_scene, "ground") ground_graph_node.name = "ground" # it is suitable to have the same name for both graphics and physics. ground_phy_node.rigid_body.composite_name="ground.comp"
def addObst_2(world): obst_2_world = xrl.createWorldFromUrdfFile("resources/urdf/env12.xml", "obst_2", [0.6, -0.2, 0.0, 1, 0, 0, 0], True, 0.1, 0.01) phy_obst_2_world = desc.simple.scene.parseColladaFile( "resources/dae/env12.dae", append_label_library=".phyobst_2", append_label_graph_meshes=".obst_2") desc.simple.graphic.addGraphicalTree(world, obst_2_world, node_name="obst_2") shpere_node = desc.core.findInTree( obst_2_world.scene.graphical_scene.root_node, "node-obst_2") comp_obst_2 = desc.simple.collision.addCompositeMesh( world, phy_obst_2_world, composite_name="obst_2.comp", offset=0.00, clean_meshes=False, ignore_library_conflicts=False) #desc.collision.copyFromGraphicalTree(comp_obst_2.root_node, obst_2_node) desc.simple.physic.addRigidBody(world, "obst_2", mass=1, contact_material="material.concrete") obst_2_position = lgsm.Displacementd() #obst_2_position.setTranslation(lgsm.vector(0.6,-0.2,0.0)) obst_2_position.setTranslation(lgsm.vector(2.6, 3.2, 0.0)) desc.simple.physic.addFixedJoint(world, "obst_2.joint", "obst_2", obst_2_position) #Binding graph, phy and coll object obst_2_graph_node = desc.core.findInTree( world.scene.graphical_scene.root_node, "obst_2") obst_2_phy_node = desc.physic.findInPhysicalScene( world.scene.physical_scene, "obst_2") obst_2_graph_node.name = "obst_2" # it is suitable to have the same name for both graphics and physics. obst_2_phy_node.rigid_body.composite_name = "obst_2.comp"
def addSphere(world): sphere_world = xrl.createWorldFromUrdfFile("resources/urdf/sphere.xml", "sphere", [0, 0, 0.1, 1, 0, 0, 0], False, 0.1, 0.01) # , "material.concrete") phy_sphere_world = desc.simple.scene.parseColladaFile( "resources/dae/sphere.dae", append_label_library=".physphere", append_label_graph_meshes=".sphere") desc.simple.graphic.addGraphicalTree(world, sphere_world, node_name="sphere") shpere_node = desc.core.findInTree( sphere_world.scene.graphical_scene.root_node, "node-sphere") comp_sphere = desc.simple.collision.addCompositeMesh( world, phy_sphere_world, composite_name="sphere.comp", offset=0.00, clean_meshes=False, ignore_library_conflicts=False) #desc.collision.copyFromGraphicalTree(comp_sphere.root_node, sphere_node) desc.simple.physic.addRigidBody(world, "sphere", mass=1, contact_material="material.concrete") sphere_position = lgsm.Displacementd() sphere_position.setTranslation(lgsm.vector(0.7, 5.3, 5.3)) desc.simple.physic.addFixedJoint(world, "sphere.joint", "sphere", sphere_position) #Binding graph, phy and coll object sphere_graph_node = desc.core.findInTree( world.scene.graphical_scene.root_node, "sphere") sphere_phy_node = desc.physic.findInPhysicalScene( world.scene.physical_scene, "sphere") sphere_graph_node.name = "sphere" # it is suitable to have the same name for both graphics and physics. sphere_phy_node.rigid_body.composite_name = "sphere.comp"
def updateHook(self): model = self.model q,qok = self.q_port.read() qdot, qdotok = self.qdot_port.read() qdes, qdesok = self.qdes_port.read() d, dok = self.d_port.read() t, tok = self.t_port.read() self.qdes = qdes #tau = np.zeros((model.nbDofs(),1)) tau = lgsm.vector([0] * model.nbInternalDofs()) if qok and qdotok and dok and tok and qdesok: # Refresh dynamic and kinematic models model.setFreeFlyerPosition(d) model.setFreeFlyerVelocity(t) model.setJointPositions(q) model.setJointVelocities(qdot) N = model.nbDofs() Na = model.nbInternalDofs() # actuated dof # Control q = self.model.getJointPositions() v = self.model.getJointVelocities() fc = self.kp * (qdes - q) + self.kd * v if self.enable_gravity_comp: tau = model.getGravityTerms() if self.enable_pos_control: tau = tau + fc self.tau_port.write(tau)
def updateHook(self): if self.model is None: return q,qok = self.q_port.read() qdot, qdotok = self.qdot_port.read() d, dok = self.d_port.read() t, tok = self.t_port.read() tau = lgsm.vector([0] * self.model.nbInternalDofs()) if qok and qdotok and dok and tok: model = self.model # Refresh dynamic and kinematic models model.setFreeFlyerPosition(d) model.setFreeFlyerVelocity(t) model.setJointPositions(q) model.setJointVelocities(qdot) # Control tau = model.getGravityTerms() self.tau_port.write(tau)
def doUpdate(self): time.sleep(0.001) # simulate a short time operation # time.sleep(0.1) # simulate a time-consumming operation tau = lgsm.vector([8, 4, 1]) self.tau_out.write(tau)
def updateHook(self): H_b_c = lgsm.Displacementd(0.0, 0.0, 0.0, 1, 0, 0, 0) H_b_c.setTranslation(lgsm.vector([0,0,0])) self.pos_out.write(H_b_c)
model.meshes.extend(kukaWorld.library.meshes) model.mechanism.CopyFrom(kukaWorld.scene.physical_scene.mechanisms[0]) model.composites.extend(kukaWorld.scene.physical_scene.collision_scene.meshes) dynmodel = physicshelper.createDynamicModel(model) control.s.setDynModel(str(dynmodel.this.__long__())) #create connectors to get robot k1g state 'k1g_q', 'k1g_qdot', 'k1g_Hroot', 'k1g_Troot', 'k1g_H' robot_name = "k1g" wm.phy.s.Connectors.OConnectorRobotState.new("ocpos"+robot_name, robot_name+"_", robot_name) wm.phy.s.Connectors.IConnectorRobotJointTorque.new("ict"+robot_name, robot_name+"_", robot_name) wm.phy.getPort(robot_name+"_q").connectTo(control.getPort("q")) wm.phy.getPort(robot_name+"_qdot").connectTo(control.getPort("qdot")) wm.phy.getPort(robot_name+"_Troot").connectTo(control.getPort("t")) wm.phy.getPort(robot_name+"_Hroot").connectTo(control.getPort("d")) control.getPort("tau").connectTo(wm.phy.getPort(robot_name+"_tau")) # Configure the robot import lgsm kuka = wm.phy.s.GVM.Robot("k1g") kuka.enableGravity(True) kuka.setJointPositions(lgsm.vector([0.4]*7)) kuka.setJointVelocities(lgsm.vector([0.0]*7)) control.s.setPeriod(TIME_STEP) control.s.start() wm.startAgents()
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 ctrl.s.start() wm.startAgents() wm.phy.s.agent.triggerUpdate() #import xdefw.interactive #xdefw.interactive.shell_console()()
#### WEIGHTS OF THE TASKS Weight_head_stabilization_task = 0.05 Weight_waist_task = 0.4 Weight_COM_task = 1.0 Weight_hand_task = 1.0 Weight_hand_task_or = 4 # weight of the force task = 1.0 by default because we don't know how to weight it wrt the other tasks Weight_force_task = 1.0 #### FORCE TASK # the desired force is expressed in the world frame force_desired = lgsm.vector(0,0,0,0,0,-5) # mux, muy, muz, fx, fy,fz is_expressed_in_world_frame = True ########### CONTROLLER ############### # formulation of the control problem # False= use full problem (ddq, Fext, tau) => more stable, but slower (not inverse of mass matrix) # True= use reduced problem (Fext, tau) => faster use_reduced_problem_no_ddq = False # solver for the quadratic problem # qld = more accurate solver = "qld" # "quadprog" ###################### PARAMETERS ##############################
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.
def doUpdate(self): time.sleep(0.5) tau = lgsm.vector([8, 4, 1]) self.tau_out.write(tau)
##### CTRL import xde_isir_controller as xic ctrl = xic.ISIRController(dynModel, rname, wm.phy, wm.icsync, "quadprog", True) gposdes = 1.2 * lgsm.ones(N) gveldes = lgsm.zeros(N) #fullTask = ctrl.createFullTask("full", 0.001) #fullTask.setKpKd(20) #fullTask.update(gposdes, gveldes) torqueTask = ctrl.createTorqueTask("torque", [1], w=1.) torqueTask.set_tau(lgsm.vector([0.11])) ##### OBSERVERS jpobs = ctrl.add_updater(xic.observers.JointPositionsObserver(ctrl.getModel())) tpobs = ctrl.add_updater(xic.observers.TorqueObserver(ctrl)) ##### SIMULATE ctrl.s.start() wm.startAgents() wm.phy.s.agent.triggerUpdate() #import xdefw.interactive #xdefw.interactive.shell_console()()
wm.createAllAgents(dt, lmd_max=.5) wm.resizeWindow("mainWindow", 640, 480, 1000, 50) ##### 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)
ctrl.createContactTask("CLF0", rname+".l_foot", lgsm.Displacement([-.039,-.027,-.031]+ RotLZdown.tolist()), 1.5) ctrl.createContactTask("CLF1", rname+".l_foot", lgsm.Displacement([-.039, .027,-.031]+ RotLZdown.tolist()), 1.5) ctrl.createContactTask("CLF2", rname+".l_foot", lgsm.Displacement([-.039, .027, .099]+ RotLZdown.tolist()), 1.5) ctrl.createContactTask("CLF3", rname+".l_foot", lgsm.Displacement([-.039,-.027, .099]+ RotLZdown.tolist()), 1.5) ctrl.createContactTask("CRF0", rname+".r_foot", lgsm.Displacement([-.039,-.027, .031]+ RotRZdown.tolist()), 1.5) ctrl.createContactTask("CRF1", rname+".r_foot", lgsm.Displacement([-.039, .027, .031]+ RotRZdown.tolist()), 1.5) ctrl.createContactTask("CRF2", rname+".r_foot", lgsm.Displacement([-.039, .027,-.099]+ RotRZdown.tolist()), 1.5) ctrl.createContactTask("CRF3", rname+".r_foot", lgsm.Displacement([-.039,-.027,-.099]+ RotRZdown.tolist()), 1.5) ##### SET TASK CONTROLLERS ref_timeline, trajz = get_sin_traj(.55, .02, 5., 0, 0, 30., dt) waistTraj = [ (lgsm.Displacement(0,0,z,1,0,0,0), lgsm.Twist(lgsm.vector(0,0,0,0,0,vz)), lgsm.Twist(lgsm.vector(0,0,0,0,0,az))) for z,vz,az in trajz] ctrl.add_updater( xic.task_controller.TrajectoryTracking(waistTask, waistTraj) ) ##### OBSERVERS fpobs = ctrl.add_updater(xic.observers.FramePoseObserver(ctrl.getModel(), rname+'.waist', lgsm.Displacement())) ##### SIMULATE ctrl.s.start() wm.startAgents() wm.phy.s.agent.triggerUpdate() #import xdefw.interactive #xdefw.interactive.shell_console()()
N = robot.getJointSpaceDim() dynModel = xrl.getDynamicModelFromWorld(robotWorld) #import xdefw.interactive #xdefw.interactive.shell_console()() ##### SET INTERACTION wm.ms.setContactLawForMaterialPair("material.metal", "material.metal", 1, 1.5) robot.enableContactWithBody("moving_wall.moving_wall", True) wm.contact.showContacts([(b,"moving_wall.moving_wall") for b in robot.getSegmentNames()]) # to display contact ##### INIT ROBOT & MODEL qinit = lgsm.vector([0,0,0,-lgsm.math.pi/2.,0,0,0]) robot.setJointPositions(qinit) dynModel.setJointPositions(qinit) robot.setJointVelocities(lgsm.zeros(N)) dynModel.setJointVelocities(lgsm.zeros(N)) ##### CTRL import xde_isir_controller as xic ctrl_moving_wall = xic.ISIRController(dynModel_moving_wall, "moving_wall", wm.phy, wm.icsync, "quadprog") gposdes = lgsm.zeros(1) gveldes = lgsm.zeros(1) fullTask = ctrl_moving_wall.createFullTask("zero_wall", w=1., kp=20) # create full task with a very low weight for reference posture fullTask.set_q(gposdes)
def doUpdate(self): tau = lgsm.vector([8, 4, 1]) self.tau_out.write(tau)
##### SET TASK CONTROLLERS RotLZUp = lgsm.Quaternion(-sqrt2on2,0.0,-sqrt2on2,0.0) * lgsm.Quaternion(0.0,0.0,0.0,1.0) RotRZUp = lgsm.Quaternion(0.0, sqrt2on2,0.0, sqrt2on2) * lgsm.Quaternion(0.0,0.0,0.0,1.0) H_lf_sole = lgsm.Displacement([-.039, 0, .034]+RotLZUp.tolist() ) H_rf_sole = lgsm.Displacement([-.039, 0,-.034]+RotRZUp.tolist() ) walkingActivity = xic.walk.WalkingActivity( ctrl, dt, rname+".l_foot", H_lf_sole, l_contacts, rname+".r_foot", H_rf_sole, r_contacts, rname+'.waist', lgsm.Displacement(0,0,0,0,0,0,1), lgsm.Displacement(0,0,.58,1,0,0,0), H_0_planeXY=lgsm.Displacement(0,0,0.002,1,0,0,0), weight=10., contact_as_objective=True) #walkingActivity.stayIdle() inf_traj = get_infinity_traj(lgsm.vector(0.,0.), lgsm.vector(1,0), R=.5, go_left=True) zmp_ref = walkingActivity.followTrajectory(inf_traj) ##### OBSERVERS zmplipmpobs = ctrl.add_updater( xic.observers.ZMPLIPMPositionObserver(ctrl.getModel(), lgsm.Displacement(), dt, 9.81) ) ##### SIMULATE ctrl.s.start() wm.startAgents() wm.phy.s.agent.triggerUpdate() walkingActivity.wait_for_end_of_walking()
b.setPrincipalInertiaFrame(lgsm.Displacement()) # inertia frame bodies.append(b) ##### Create all joints and configure joints_name = ["j_root", "j_1", "j_2", "j_3"] joints = [] # ists of joint instances joints.append(phy.s.GVM.FixedJoint.new(joints_name[0])) joints[0].configure(lgsm.Displacement()) # Set position of fixed joint for j_name in joints_name[1:]: j = phy.s.GVM.HingeJoint.new(j_name) T_pc = lgsm.Displacement() T_pc.setTranslation(lgsm.vector(.5,0,0)) j.configure(T_pc, # Displacement from parent body to child body lgsm.vector(0,0,0), # Hinge center position in parent frame lgsm.vector(0,1,0), # Hinge axis in parent frame 0.2) # Hinge angular offset j.setJointDampingCoeff(2) # add some damping j.enableJointDamping() joints.append(j) ##### Create connection between bodies with joints ms.addRigidBodyToGround(bodies[0], joints[0]) for i in range(1, len(bodies)): ms.addRigidBody(bodies[i-1], bodies[i], joints[i])
H_rf_sole = lgsm.Displacement([-.039, 0,-.034]+RotRZUp.tolist() ) walkingActivity = xic.walk.WalkingActivity( ctrl, dt, rname+".l_foot", H_lf_sole, l_contacts, rname+".r_foot", H_rf_sole, r_contacts, rname+'.waist', lgsm.Displacement(0,0,0,0,0,0,1), lgsm.Displacement(0,0,.58,0,0,0,1), H_0_planeXY=lgsm.Displacement(0,0,0.002,1,0,0,0), weight=10., contact_as_objective=True) zmp_ref = walkingActivity.goTo([-1.5,0.]) ##### OBSERVERS zmplipmpobs = ctrl.add_updater( xic.observers.ZMPLIPMPositionObserver(ctrl.getModel(), lgsm.Displacement(), dt, 9.81) ) #if fixed camera cam_traj = [xic.observers.lookAt(lgsm.vector(-2,1.5,1.5), lgsm.vector(-0.5,0,0.6), lgsm.vector(0,0,1))] # or with a moving camera R, W, P = 2., 2*pi/7., pi/4. cam_traj = [] for tt in lgsm.np.arange(0, 11, dt): x = -0.5 + R*lgsm.np.cos(tt*W+P) y = + R*lgsm.np.sin(tt*W+P) cam_traj.append(xic.observers.lookAt(lgsm.vector(x,y,1.5), lgsm.vector(-0.5,0,0.6), lgsm.vector(0,0,1))) screenobs = ctrl.add_updater(xic.observers.ScreenShotObserver(wm, "rec", cam_traj=cam_traj)) ##### SIMULATE ctrl.s.start()
####### WEIGHTS OF THE TASKS ##################### Weight_head_stabilization_task = 0.05 Weight_waist_task = 0.4 Weight_COM_task = 1.0 Weight_hand_task = 1.0 Weight_hand_task_or = 4 # weight of the force task = 1.0 by default because we don't know how to weight it wrt the other tasks Weight_force_task = 1.0 #### FORCE TASK # the desired force is expressed in the world frame force_desired = lgsm.vector(0,0,0,-10,0,0) # mux, muy, muz, fx, fy,fz is_expressed_in_world_frame = True ########### CONTROLLER ############### # formulation of the control problem # False= use full problem (ddq, Fext, tau) => more stable, but slower (not inverse of mass matrix) # True= use reduced problem (Fext, tau) => faster use_reduced_problem_no_ddq = False # solver for the quadratic problem # qld = more accurate solver = "qld" # "quadprog" ###################### PARAMETERS ##############################
def RL(myList, tol=10): return [round(v, tol) for v in myList] if hasattr(myList, "__len__") else round(myList, tol) ##### modfile += """ def get_bodies_data(): \"\"\" Get bodies mass, inertia and displacement from parent frame. Returns a dictionnary {'body name': (mass, moments_of_inertia, H_parent_body)}. \"\"\" return { """ for b in body_mass: Disp_parent_body = lgsm.Displacement(lgsm.vector(H_parentCoM_bCoM[b][0:3,3]), lgsm.Quaternion(H_parentCoM_bCoM[b][0:3,0:3])) modfile += " '{0}': ({1}, {2}, lgsm.Displacement({3})),\n".format(b, RL(body_mass[b]), RL(body_moment_of_inertia[b]), RL(Disp_parent_body.tolist()))#H_parentCoM_bCoM[b]) modfile += " }\n\n" ##### modfile += """ def get_joints_data(): \"\"\" Get joints data. Returns a dictionnary {'joints name': ('parent body name', 'child body name', Hinge_center_in_parent_frame,