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