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.),)
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)
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"]