def doUpdate(self, q, qdot):
model = self.model
model.setJointPositions(q)
model.setJointVelocities(qdot)
H6 = model.getSegmentPosition(model.getSegmentIndex("p1_b_3"))
H = lgsm.Displacement()
H.setTranslation(lgsm.vectord(0,0,0))
H.setRotation(H6.getRotation())
J66 = model.getSegmentJacobian(model.getSegmentIndex("p1_b_3"))
J60 = H.adjoint() * J66
T60 = H.adjoint() * model.getSegmentVelocity(model.getSegmentIndex("p1_b_3"))
Xdes = lgsm.Displacement(.3,.3,.3,1,0,0,0)
xd = Xdes.getTranslation() # desired position of the effector
x = H6.getTranslation()
v = T60.getLinearVelocity()
fc = self.kp * (xd - x) - self.kd * v
#tau = lgsm.vector([0] * model.nbInternalDofs())
tau = J60[3:6,:].transpose() * fc
tau += model.getGravityTerms()
self.tau_out.write(tau)
def getCompositePairLocalDisplacement(self, *args):
"""
Return a pair of displacement that represent the position of the closest point that belongs
to a pair of body in the body frame
:param body1_name: string
:param body2_name: string
:return: ai_displacement, aj_displacement
:param composite_pair_desc: a composite pair descriptor
"""
if len(args) == 2: #arguments are body_name string
if type(args[0]) == str:
body1_name = args[0]
else:
raise TypeError("First argument is a string(body1_name)")
if type(args[1]) == str:
body2_name = args[1]
else:
raise TypeError("Second argument is a string(body2_name)")
compPairDesc = self.createCompositePairDescriptor(body1_name, body2_name)
elif len(args) == 1: #argument is a composite_pair_desc
if str(type(args[0])) == "<class 'xdefw.rtt.CompositePairDescriptor'>":
compPairDesc = args[0]
else:
raise TypeError("Argument is a xdefw.rtt.CompositePairDescriptor")
else:
raise TypeError("Wrong number of arguments, usage: getCompositePairLocalDisplacement(body1_name, body2_name) getCompositePairLocalDisplacement(composite_pair_desc)")
ai = compPairDesc.getGlobalMinDist().ai
ni = compPairDesc.getGlobalMinDist().ni
ai_displacement = lgsm.Displacementd()
ai_displacement.setTranslation(lgsm.vectord(ai[0], ai[1], ai[2]))
ai_displacement.setRotation(alignz(lgsm.vectord(ni[0], ni[1], ni[2])))
aj = compPairDesc.getGlobalMinDist().aj
nj = compPairDesc.getGlobalMinDist().nj
aj_displacement = lgsm.Displacementd()
aj_displacement.setTranslation(lgsm.vectord(aj[0], aj[1], aj[2]))
aj_displacement.setRotation(alignz(lgsm.vectord(nj[0], nj[1], nj[2])))
return ai_displacement, aj_displacement
def alignz(vector):
"""
Return the rotation matrix necessary to align the z axis of the 0-frame with a vector
:param vector: lgsm.vectord of size 3
:return : lgsm.Rotation3
"""
angle = np.arccos(vector[2]) #angle vec/Z
axe = np.cross([0,0,1],vector.transpose()) #find rotation axis from Z to vec
if lgsm.norm(axe) < 0.001: #Check if Z and vec are parallel
if vector[2] < 0: #use X axis as rotation axis for Z to vec and 0/pi given vec_z direction
R = lgsm.Rotation3.fromAxisAngle(lgsm.vectord([1,0,0]), math.pi)
else:
R = lgsm.Rotation3.fromAxisAngle(lgsm.vectord([1,0,0]), 0)
else:
R = lgsm.Rotation3.fromAxisAngle(axe.transpose(), angle)
return R
import rtt_interface
import agents.graphic.simple
graph = agents.graphic.simple.createAgent("graphic", 0)
p1 = graph.addCreateOutputPort("v1", "VectorXd")
p2 = graph.addCreateOutputPort("v2", "VectorXd")
p3 = graph.addCreateOutputPort("p3", "Displacementd")
p1_in = graph.addCreateInputPort("v1_in", "VectorXd", True)
p2_in = graph.addCreateInputPort("v2_in", "VectorXd", True)
p3_in = graph.addCreateInputPort("p3_in", "Displacementd", True)
sTask = xdeTasksUtils.synchronizer.createTask()
v1 = lgsm.vectord(1, 2, 3)
v2 = lgsm.vectord(1, 2, 4, 9, 2)
d3 = lgsm.Displacementd()
p1.connectTo(p1_in)
p2.connectTo(p2_in)
p3.connectTo(p3_in)
sTask.addPort(p1_in)
sTask.addPort(p2_in)
sTask.addPort(p3_in)
xdeTasksUtils.synchronizer.startTask()
p1.write(v1)
p2.write(v2)
############################### Controller & Dynamic model ###############################
# dsimi.rtt : convert RTT::TaskContext * into a python object /
control = xdefw.rtt.Task(ddeployer.load("control", "XDE_SimpleController", "XDESimpleController-gnulinux", "", libdir="/home/anis/ros_workspace/kuka_controller/lib/orocos/gnulinux"))
kukaModel = xde.desc.physic.physic_pb2.MultiBodyModel()
kukaModel.kinematic_tree.CopyFrom(world.scene.physical_scene.nodes[0])
kukaModel.meshes.extend(world.library.meshes)
kukaModel.mechanism.CopyFrom(world.scene.physical_scene.mechanisms[0])
kukaModel.composites.extend(world.scene.physical_scene.collision_scene.meshes)
model = physicshelper.createDynamicModel(kukaModel)
#model.setJointPositions(lgsm.vectord(-0.5, -0.3, -0.4, -0.4, -0.2, -0.2, -0.2))
#model.setJointPositions(lgsm.vectord(0.05, 0.68, 0.68, 0.68, 0.68, 0.68, 0.68))
#model.setJointPositions(lgsm.vectord(3.5, 0.68, -0.5, 0.68, 0.68, 0.68, 0.68))
model.setJointPositions(lgsm.vectord(0.5, 0.68, 0.0, 1.5, 0, 0.0, 0.0)) #Position articulaire de depart du robot
#model.setJointPositions(lgsm.vectord(0, 3.14159265359/2, 0, 0, 0, 0.0, 0.0))
#model.setJointPositions(lgsm.vectord(0, 0, 0, 0, 0, 0.0, 0.0))
#max 2.97 2.09 2.97 2.09 2.97 2.09 2.9
#model.setJointPositions(lgsm.vectord(0, 0.0, 0.0, 0, 0, 0.0, -1.5))
#0, 0.0, 0.0, 3.14159265/2, 0, 0.0, 0.0
#model.setJointPositions(lgsm.vectord(0.105, 0.7755, 1.11976, 1.23, 0.02420, 1.1028, 0.3))
parameters.kuka = wm.phy.s.GVM.Robot("kuka")
wm.addMarkers(world, ["kuka.01"], thin_markers=False)
#wm.graph_scn.SceneInterface.setNodeMaterial('kuka.01', 'xde/BlueTransparentSet', True) # Pour changer la couleur du mannequin
#wm.graph_scn.SceneInterface.setNodeMaterial('kuka.02', 'xde/RedTransparentSet', True) # Pour changer la couleur du mannequin
#wm.graph_scn.SceneInterface.setNodeMaterial('kuka.03', 'xde/GreenTransparentSet', True) # Pour changer la couleur du mannequin
"",
libdir="/home/anis/ros_workspace/kuka_controller/lib/orocos/gnulinux"))
kukaModel = xde.desc.physic.physic_pb2.MultiBodyModel()
kukaModel.kinematic_tree.CopyFrom(world.scene.physical_scene.nodes[0])
kukaModel.meshes.extend(world.library.meshes)
kukaModel.mechanism.CopyFrom(world.scene.physical_scene.mechanisms[0])
kukaModel.composites.extend(world.scene.physical_scene.collision_scene.meshes)
model = physicshelper.createDynamicModel(kukaModel)
#model.setJointPositions(lgsm.vectord(-0.5, -0.3, -0.4, -0.4, -0.2, -0.2, -0.2))
#model.setJointPositions(lgsm.vectord(0.05, 0.68, 0.68, 0.68, 0.68, 0.68, 0.68))
#model.setJointPositions(lgsm.vectord(3.5, 0.68, -0.5, 0.68, 0.68, 0.68, 0.68))
model.setJointPositions(
lgsm.vectord(2.1, 0.68, 0.0, 1.5, 0, 0.0,
0.0)) #Position articulaire de depart du robot
#model.setJointPositions(lgsm.vectord(0, 3.14159265359/2, 0, 0, 0, 0.0, 0.0))
#model.setJointPositions(lgsm.vectord(0, 0, 0, 0, 0, 0.0, 0.0))
#max 2.97 2.09 2.97 2.09 2.97 2.09 2.9
#model.setJointPositions(lgsm.vectord(0, 0.0, 0.0, 0, 0, 0.0, -1.5))
#0, 0.0, 0.0, 3.14159265/2, 0, 0.0, 0.0
#model.setJointPositions(lgsm.vectord(0.105, 0.7755, 1.11976, 1.23, 0.02420, 1.1028, 0.3))
parameters.kuka = wm.phy.s.GVM.Robot("kuka")
wm.addMarkers(world, ["kuka.01"], thin_markers=False)
#wm.graph_scn.SceneInterface.setNodeMaterial('kuka.01', 'xde/BlueTransparentSet', True) # Pour changer la couleur du mannequin
#wm.graph_scn.SceneInterface.setNodeMaterial('kuka.02', 'xde/RedTransparentSet', True) # Pour changer la couleur du mannequin
#wm.graph_scn.SceneInterface.setNodeMaterial('kuka.03', 'xde/GreenTransparentSet', True) # Pour changer la couleur du mannequin
