def __init__(self, taskName, world, robotName):
task = rtt_interface.PyTaskFactory.CreateTask(taskName)
xdefw.rtt.Task.__init__(self, task)
# model instance
multiBodyModel = xde.desc.physic.physic_pb2.MultiBodyModel()
mechanism_index = 0
for m in world.scene.physical_scene.mechanisms:
if robotName == m.name:
break
else:
mechanism_index = mechanism_index + 1
multiBodyModel.kinematic_tree.CopyFrom(world.scene.physical_scene.nodes[ mechanism_index ])
multiBodyModel.meshes.extend(world.library.meshes)
multiBodyModel.mechanism.CopyFrom(world.scene.physical_scene.mechanisms[ mechanism_index ])
multiBodyModel.composites.extend(world.scene.physical_scene.collision_scene.meshes)
self.model = physicshelper.createDynamicModel(multiBodyModel)
# create input ports
self.c_in = self.addCreateInputPort("cont", "SMsg", True)
self.c_ok = False
self.c = None
self.tau_out = self.addCreateOutputPort("tau", "VectorXd")
def __init__(self, taskName, world, robotName):
task = rtt_interface.PyTaskFactory.CreateTask(taskName)
xdefw.rtt.Task.__init__(self, task)
# model instance
multiBodyModel = xde.desc.physic.physic_pb2.MultiBodyModel()
mechanism_index = 0
for m in world.scene.physical_scene.mechanisms:
if robotName == m.name:
break
else:
mechanism_index = mechanism_index + 1
multiBodyModel.kinematic_tree.CopyFrom(world.scene.physical_scene.nodes[ mechanism_index ])
multiBodyModel.meshes.extend(world.library.meshes)
multiBodyModel.mechanism.CopyFrom(world.scene.physical_scene.mechanisms[ mechanism_index ])
multiBodyModel.composites.extend(world.scene.physical_scene.collision_scene.meshes)
self.model = physicshelper.createDynamicModel(multiBodyModel)
# create input ports
# Those ports will be used to get the current state of the robot
self.q_in = self.addCreateInputPort("q", "VectorXd", True)
self.q_ok = False
self.qdot_in = self.addCreateInputPort("qdot", "VectorXd", True)
self.qdot_ok = False
# create output ports
# Those ports will be used to send the command to the robot
self.q_des_out = self.addCreateOutputPort("q_des", "VectorXd")
self.qdot_des_out = self.addCreateOutputPort("qdot_des", "VectorXd")
self.kp_des_out = self.addCreateOutputPort("kp_des", "VectorXd")
self.kd_des_out = self.addCreateOutputPort("kd_des", "VectorXd")
self.kp_des = 100
self.kd_des = 20
def __init__(self, taskName, world, robotName):
task = rtt_interface.PyTaskFactory.CreateTask(taskName)
xdefw.rtt.Task.__init__(self, task)
multiBodyModel = xde.desc.physic.physic_pb2.MultiBodyModel()
mechanism_index = 0
for m in world.scene.physical_scene.mechanisms:
if robotName == m.name:
break
else:
mechanism_index = mechanism_index + 1
multiBodyModel.kinematic_tree.CopyFrom(world.scene.physical_scene.nodes[ mechanism_index ])
multiBodyModel.meshes.extend(world.library.meshes)
multiBodyModel.mechanism.CopyFrom(world.scene.physical_scene.mechanisms[ mechanism_index ])
multiBodyModel.composites.extend(world.scene.physical_scene.collision_scene.meshes)
self.model = physicshelper.createDynamicModel(multiBodyModel)
# Input port to read the current state of the robot
self.q_in = self.addCreateInputPort("q", "VectorXd", True)
self.q_ok = False
# Input port to read the current state of the robot
self.qdot_in = self.addCreateInputPort("qdot", "VectorXd", True)
self.qdot_ok = False
# Output port to write torque commands
self.tau_out = self.addCreateOutputPort("tau", "VectorXd")
self.kp = 100
self.kd = 20
def connectToRobot(self, phy, world, robot_name):
self.model = physicshelper.createDynamicModel(world, robot_name)
#create connectors to get robot k1g state 'k1g_q', 'k1g_qdot', 'k1g_Hroot', 'k1g_Troot', 'k1g_H'
phy.s.Connectors.OConnectorRobotState.new("ocpos"+robot_name, robot_name+"_", robot_name)
phy.s.Connectors.IConnectorRobotJointTorque.new("ict"+robot_name, robot_name+"_", robot_name)
phy.getPort(robot_name+"_q").connectTo(self.getPort("q"))
phy.getPort(robot_name+"_qdot").connectTo(self.getPort("qdot"))
phy.getPort(robot_name+"_Troot").connectTo(self.getPort("t"))
phy.getPort(robot_name+"_Hroot").connectTo(self.getPort("d"))
self.getPort("tau").connectTo(phy.getPort(robot_name+"_tau"))
def connectToRobot(self, phy, world, robot_name):
multiBodyModel = xde.desc.physic.physic_pb2.MultiBodyModel()
multiBodyModel.kinematic_tree.CopyFrom(world.scene.physical_scene.nodes[0])
multiBodyModel.meshes.extend(world.library.meshes)
multiBodyModel.mechanism.CopyFrom(world.scene.physical_scene.mechanisms[0])
multiBodyModel.composites.extend(world.scene.physical_scene.collision_scene.meshes)
self.model = physicshelper.createDynamicModel(multiBodyModel)
#create connectors to get robot k1g state 'k1g_q', 'k1g_qdot', 'k1g_Hroot', 'k1g_Troot', 'k1g_H'
phy.s.Connectors.OConnectorRobotState.new("ocpos"+robot_name, robot_name+"_", robot_name)
phy.s.Connectors.IConnectorRobotJointTorque.new("ict"+robot_name, robot_name+"_", robot_name)
phy.getPort(robot_name+"_q").connectTo(self.getPort("q"))
phy.getPort(robot_name+"_qdot").connectTo(self.getPort("qdot"))
phy.getPort(robot_name+"_Troot").connectTo(self.getPort("t"))
phy.getPort(robot_name+"_Hroot").connectTo(self.getPort("d"))
self.getPort("tau").connectTo(phy.getPort(robot_name+"_tau"))
kukaWorld = xrl.createWorldFromUrdfFile(xr.kuka, "k1g", [0,0,0.4, 1, 0, 0, 0], True, 1, 0.005) #, "material.concrete")
wm.addMarkers(sphereWorld, ["sphere.sphere"], thin_markers=False)
wm.addWorld(sphereWorld)
wm.addWorld(kukaWorld)
#Controller
control = xdefw.rtt.Task(ddeployer.load("control", "XDE_SimpleController", "XDESimpleController-gnulinux", "", libdir="/tmp/XDE-SimpleController/_build/src/"))
import xde.desc.physic.physic_pb2
model = xde.desc.physic.physic_pb2.MultiBodyModel()
model.kinematic_tree.CopyFrom(kukaWorld.scene.physical_scene.nodes[0])
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"))
kukacommon.addCollision_with_obst_2(world)
wm.addWorld(world)
#wm.graph_scn.SceneInterface.setNodeMaterial('ground', 'xde/ConcretGround', True) # Pour changer la couleur du mannequin
############################### 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))
