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,
