Esempio n. 1
0
from dynamic_graph.sot.youbot.robot import youbot
from dynamic_graph.sot.pr2.robot import Pr2
from dynamic_graph.ros.robot_model import RosRobotModel
from dynamic_graph.sot.core import RobotSimu, FeaturePosition, Task, SOT
from dynamic_graph.sot.core.meta_tasks import generic6dReference
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug, writeGraph
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph.sot.dyninv import TaskInequality, TaskJointLimits
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import SolverKine
robot = youbot('youbot', device=RobotSimu('youbot'))
#robot = Pr2('youbot', device=RobotSimu('youbot'))
dimension = robot.dynamic.getDimension()
robot.device.resize (dimension)
from dynamic_graph.ros import Ros
ros = Ros(robot)
#waisttask-1

robot_pose = ((1.,0,0,0.4),
    (0,1.,0,0),
    (0,0,1.,0.0),
    (0,0,0,1.),)	
feature_waist = FeaturePosition ('position_waist', robot.dynamic.base_joint,robot.dynamic.Jbase_joint, robot_pose)
task_waist = Task ('waist_task')
task_waist.controlGain.value = 0.5
task_waist.add (feature_waist.name)

#waisttask
task_waist_metakine=MetaTaskKine6d('task_waist_metakine',robot.dynamic,'base_joint','base_joint')
goal_waist = ((1.,0,0,0.0),(0,1.,0,-0.0),(0,0,1.,0.0),(0,0,0,1.),)
Esempio n. 2
0
    def buildurrobot(self):
        # collision components

        self.robot = youbot(name="robot", device=RobotSimu("youbot"))
        self.dimension = self.robot.dynamic.getDimension()
        plug(self.robot.device.state, self.robot.dynamic.position)

        self.a = sc.SotCollision("sc")
        # create links for collision check
        self.a.createcollisionlink("wall1", "box", "external", (2.04, 0.015, 0.3, 0, 0, 0.0, 0.0, 0, 0))
        self.a.createcollisionlink("wall2", "box", "external", (0.015, -2.44, 0.3, 0, 0, 0, 0, 0, 0))
        self.a.createcollisionlink("wall3", "box", "external", (2.04, 0.015, 0.3, 0, 0, 0.0, 0.0, 0, 0))
        self.a.createcollisionlink("platform1", "box", "external", (0.5, 0.8, 0.11, 0.0, 0.0, 0.0, 0, 0.0, 0))
        self.a.createcollisionlink("platform2", "box", "external", (0.5, 0.8, 0.11, 0.0, 0.0, 0.0, 0, 0.0, 0))
        # self.a.createcollisionlink("base_link","box","internal",(0.64,0.4,0.11,0.0,0.0,0,0,0,0))
        self.a.createcollisionlink("base_link_1", "box", "internal", (0.1, 0.4, 0.11, 0.26, 0.0, 0, 0, 0, 0))
        self.a.createcollisionlink("base_link_2", "box", "internal", (0.1, 0.4, 0.11, -0.26, 0.0, 0, 0, 0, 0))
        self.a.createcollisionlink("base_link_3", "box", "internal", (0.44, 0.1, 0.11, 0.0, 0.15, 0, 0, 0, 0))
        self.a.createcollisionlink("base_link_4", "box", "internal", (0.44, 0.1, 0.11, 0.0, -0.15, 0, 0, 0, 0))

        # plug joint position and joint jacobian to collision checker entity
        self.a.wall1.value = ((1, 0, 0, 0), (0, 1, 0, 1.22), (0, 0, 1, 0.12), (0, 0, 0, 1))
        self.a.wall2.value = ((1, 0, 0, 1.02), (0, 1, 0, 0), (0, 0, 0, 0.12), (0, 0, 0, 1))
        self.a.wall3.value = ((1, 0, 0, 0), (0, 1, 0, -1.22), (0, 0, 1, 0.12), (0, 0, 0, 1))

        self.a.platform1.value = ((1, 0, 0, 0.77), (0, 1, 0, 0.82), (0, 0, 1, 0.025), (0, 0, 0, 1))
        self.a.platform2.value = ((1, 0, 0, 0.77), (0, 1, 0, -0.82), (0, 0, 1, 0.025), (0, 0, 0, 1))

        # plug(self.robot.dynamic.base_joint,self.a.base_link)
        plug(self.robot.dynamic.base_joint, self.a.base_link_1)
        plug(self.robot.dynamic.base_joint, self.a.base_link_2)
        plug(self.robot.dynamic.base_joint, self.a.base_link_3)
        plug(self.robot.dynamic.base_joint, self.a.base_link_4)

        self.a.Jwall1.value = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
        self.a.Jwall2.value = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
        self.a.Jwall3.value = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
        self.a.Jplatform1.value = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
        self.a.Jplatform2.value = ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
        plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link_1)
        plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link_2)
        plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link_3)
        plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link_4)

        self.a.createcollisionpair("base_link_1", "platform1")
        self.a.createcollisionpair("base_link_1", "platform2")
        self.a.createcollisionpair("base_link_2", "platform1")
        self.a.createcollisionpair("base_link_2", "platform2")
        self.a.createcollisionpair("base_link_3", "platform1")
        self.a.createcollisionpair("base_link_3", "platform2")
        self.a.createcollisionpair("base_link_4", "platform1")
        self.a.createcollisionpair("base_link_4", "platform2")

        goal = ((1.0, 0, 0, 0.5), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
        self.task_waist_metakine = MetaTaskKine6d("task_waist_metakine", self.robot.dynamic, "base_joint", "base_joint")
        # task_waist_metakine.opmodif = goal
        # task_waist_metakine.feature.frame('desired')
        self.task_waist_metakine.featureDes.position.value = goal
        self.task_waist_metakine.gain.setConstant(1)

        self.solver = SolverKine("sot")
        self.solver.setSize(self.dimension)
        # self.solver.push (self.task_waist_metakine.task.name)
        # self.solver.push (self.task_collision_avoidance.name)
        # self.solver.push (self.task_wrist_metakine.task.name)
        # self.solver.damping.value =3e-2
        plug(self.solver.control, self.robot.device.control)
Esempio n. 3
0
print("sot_youbot")
print("Prologue loaded.")

from dynamic_graph import plug
from dynamic_graph.sot.youbot.robot import youbot
from dynamic_graph.sot.core import  SOT,FeaturePosition, Task

#from dynamic_graph.sot.youbot.youbot_device import YoubotDevice
from dynamic_graph.entity import PyEntityFactoryClass

Device = PyEntityFactoryClass('YoubotDevice')
robot = youbot(name = 'youBot', device = Device('youBot_device'))
dimension = robot.dynamic.getDimension()
plug(robot.device.state, robot.dynamic.position)

#solver#
solver = SOT ('solver')
solver.setSize (robot.dynamic.getDimension())
plug (solver.control, robot.device.control)

__all__ = ["robot"]