Ejemplo n.º 1
0
    def __init__(self, robot):
        self.robot = robot
        """
        # Make sure control does not exceed joint limits.
        self.jointLimitator = JointLimitator('joint_limitator')
        plug(self.robot.dynamic.position, self.jointLimitator.joint)
        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
        """

        # Create the solver.
        self.sot = SolverKine('solver')
        self.sot.signal('damping').value = 1e-6
        self.sot.setSize(self.robot.dimension)

        # Set second order computation
        self.robot.device.setSecondOrderIntegration()
        self.sot.setSecondOrderKinematics()
        plug(self.robot.device.velocity, self.robot.dynamic.velocity)
        plug(self.robot.dynamic.velocity, self.sot.velocity)
        """
        # Plug the solver control into the filter.
        plug(self.sot.control, self.jointLimitator.controlIN)

        # Important: always use 'jointLimitator.control'
        # and NOT 'sot.control'!

        if robot.device:
            plug(self.jointLimitator.control, robot.device.control)
        
        """

        # Plug the solver control into the robot.
        plug(self.sot.control, robot.device.control)
    def __init__(self, robot):
        self.robot = robot
        """
        # Make sure control does not exceed joint limits.
        self.jointLimitator = JointLimitator('joint_limitator')
        plug(self.robot.dynamic.position, self.jointLimitator.joint)
        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
        """

        # Create the solver.
        self.sot = SolverKine('solver')
        self.sot.signal('damping').value = 1e-6
        self.sot.setSize(self.robot.dimension)

        # Set second order computation
        self.robot.device.setSecondOrderIntegration()
        self.sot.setSecondOrderKinematics()
        plug(self.robot.device.velocity, self.robot.dynamic.velocity)
        plug(self.robot.dynamic.velocity, self.sot.velocity)
        """
        # Plug the solver control into the filter.
        plug(self.sot.control, self.jointLimitator.controlIN)

        # Important: always use 'jointLimitator.control'
        # and NOT 'sot.control'!

        if robot.device:
            plug(self.jointLimitator.control, robot.device.control)
        """

        # Plug the solver control into the robot.
        plug(self.sot.control, robot.device.control)
    def __init__(self, robot):
        self.robot = robot

        # Make sure control does not exceed joint limits.
        self.jointLimitator = JointLimitator('joint_limitator')
        plug(self.robot.dynamic.position, self.jointLimitator.joint)
        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)

        # Create the solver.
        self.sot = SolverKine('solver')
        self.sot.signal('damping').value = 1e-6
        self.sot.setSize(self.robot.dimension)

        # Plug the solver control into the filter.
        plug(self.sot.control, self.jointLimitator.controlIN)

        # Important: always use 'jointLimitator.control'
        # and NOT 'sot.control'!

        if robot.device:
            plug(self.jointLimitator.control, robot.device.control)
Ejemplo n.º 4
0
    def __init__(self, robot):
        self.robot = robot

        # Make sure control does not exceed joint limits.
        self.jointLimitator = JointLimitator("joint_limitator")
        plug(self.robot.dynamic.position, self.jointLimitator.joint)
        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)

        # Create the solver.
        self.sot = SolverKine("solver")
        self.sot.signal("damping").value = 1e-6
        self.sot.setSize(self.robot.dimension)

        # Plug the solver control into the filter.
        plug(self.sot.control, self.jointLimitator.controlIN)

        # Important: always use 'jointLimitator.control'
        # and NOT 'sot.control'!

        if robot.device:
            plug(self.jointLimitator.control, robot.device.control)
Ejemplo n.º 5
0
robot.control.unplug()

# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine


def toList(sot):
    return map(lambda x: x[1:-1], sot.dispStack().split('|')[1:])


SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control, robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------

# ---- TASK GRIP ---
taskRH = MetaTaskKine6d('rh', dyn, 'rh', 'right-wrist')
handMgrip = eye(4)
handMgrip[0:3, 3] = (0, 0, -0.21)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')

taskLH = MetaTaskKine6d('lh', dyn, 'lh', 'left-wrist')
class Solver:

    def __init__(self, robot):
        self.robot = robot

        """
        # Make sure control does not exceed joint limits.
        self.jointLimitator = JointLimitator('joint_limitator')
        plug(self.robot.dynamic.position, self.jointLimitator.joint)
        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
        """

        # Create the solver.
        self.sot = SolverKine('solver')
        self.sot.signal('damping').value = 1e-6
        self.sot.setSize(self.robot.dimension)
        
        # Set second order computation
        self.robot.device.setSecondOrderIntegration()
        self.sot.setSecondOrderKinematics()
        plug(self.robot.device.velocity,self.robot.dynamic.velocity)
	plug(self.robot.dynamic.velocity,self.sot.velocity)

        """
        # Plug the solver control into the filter.
        plug(self.sot.control, self.jointLimitator.controlIN)

        # Important: always use 'jointLimitator.control'
        # and NOT 'sot.control'!

        if robot.device:
            plug(self.jointLimitator.control, robot.device.control)
        
        """

        # Plug the solver control into the robot.
        plug(self.sot.control, robot.device.control)


    def push (self, task):
        """
        Proxy method to push a task (not a MetaTask) in the sot
        """
        self.sot.push (task.name)
        if task.name!="taskposture" and "taskposture" in self.toList():
            self.sot.down("taskposture")

    def rm (self, task):
        """
        Proxy method to remove a task from the sot
        """
        self.sot.rm (task.name)

    def pop (self):
        """
        Proxy method to remove the last (usually posture) task from the sot
        """
        self.sot.pop ()

    def __str__ (self):
        return self.sot.display ()

    def toList(self):
        """
        Creates the list of the tasks in the sot
        """
        return map(lambda x: x[1:-1],self.sot.dispStack().split('|')[1:])

    def clear(self):
        """
        Proxy method to remove all tasks from the sot
        """
        self.sot.clear ()
Ejemplo n.º 7
0
def SotPr2(robot):
    SolverKine.toList = toList
    sot = SolverKine('sot')
    sot.setSize(robot.dimension)
    return sot
Ejemplo n.º 8
0
    (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.),)
task_waist_metakine.feature.frame('desired')
#task_waist_metakine.feature.selec.value = '011101'#RzRyRxTzTyTx
task_waist_metakine.gain.setConstant(0.5)
task_waist_metakine.featureDes.position.value = goal_waist
solver = SolverKine('sot_solver')
solver.setSize (robot.dynamic.getDimension())
robot.device.resize (robot.dynamic.getDimension())
'''
#taskinequality
task_waist=TaskInequality('taskcollision')
collision_feature = FeatureGeneric('collisionfeature')
plug(a.collisionJacobian,collision_feature.jacobianIN)
plug(a.collisionDistance,collision_feature.errorIN)
task_collision_avoidance.add(collision_feature.name)
task_collision_avoidance.referenceInf.value = (0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05)    
task_collision_avoidance.referenceSup.value = (2e10,2e10,2e10,2e10,2e10,2e10,2e10,2e10)  
task_collision_avoidance.dt.value=1
task_collision_avoidance.controlGain.value=50.0
'''
# ll
Ejemplo n.º 9
0
class RobotCollisionModel:
    def __init__(self):
        # Marker array for link collision model
        self.linkcollisionmodel = MarkerArray()
        # Define marker properties for link
        self.rcm_pub = rospy.Publisher("collision_model", MarkerArray)
        self.r = rospy.Rate(1)
        self.nolinks = 5
        self.dt = 0
        self.buildurrobot()

    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)

    def publish(self):
        # self.robot.dynamic.Jwrist_2_joint.recompute(self.dt)
        self.a.collisionDistance.recompute(self.dt)
        self.a.collisionJacobian.recompute(self.dt)
        # print self.a.collisionDistance.value
        self.dt = self.dt + 1
        self.getstate()
        self.rcm_pub.publish(self.linkcollisionmodel)
        self.r.sleep()
        self.robot.device.increment(0.01)

    def getstate(self):
        state = rcm.a.getfclstate()
        # print state
        # a = self.robot.dynamic.wrist_2_joint.value
        for i in range(9):
            self.link = Marker()
            self.link.type = Marker().CUBE
            # self.link.mesh_resource = "package://sot-collision/mesh/capsule/capsule.dae"
            self.link.header.frame_id = "/odom"
            self.link.header.stamp = rospy.Time.now()
            # self.link.ns = "basicshapes"
            self.link.id = i
            self.link.action = self.link.ADD
            # print state
            self.link.pose.position.x = state[i][0]
            self.link.pose.position.y = state[i][1]
            self.link.pose.position.z = state[i][2]
            self.link.pose.orientation.x = state[i][3]
            self.link.pose.orientation.y = state[i][4]
            self.link.pose.orientation.z = state[i][5]
            self.link.pose.orientation.w = state[i][6]
            self.link.scale.x = state[i][7]
            self.link.scale.y = state[i][8]
            self.link.scale.z = state[i][9]
            self.link.color.r = 0.0
            self.link.color.g = 1.0
            self.link.color.b = 0.0
            self.link.color.a = 1.0
            # pself.link.pose.orientation
            # self.link.lifetime =  rospy.Duration();
            self.linkcollisionmodel.markers.append(self.link)
Ejemplo n.º 10
0
taskPosture = Task('taskPosture')
taskPosture.add('featurePosture')
plug(taskPosture.error,gainPosture.error)
plug(gainPosture.gain,taskPosture.controlGain)

# --- TASK RIGHT FOOT
# Task right hand
#plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
#plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 5

# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)

# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
from dynamic_graph.tracer_real_time import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')

#tr.add(dyn.name+'.ffposition','ff')
tr.add(taskRF.featureDes.name+'.position','refr')
tr.add(taskLF.featureDes.name+'.position','refl')
tr.add('dyn.rf','r')
Ejemplo n.º 11
0
class Solver:
    def __init__(self, robot):
        self.robot = robot
        """
        # Make sure control does not exceed joint limits.
        self.jointLimitator = JointLimitator('joint_limitator')
        plug(self.robot.dynamic.position, self.jointLimitator.joint)
        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
        """

        # Create the solver.
        self.sot = SolverKine('solver')
        self.sot.signal('damping').value = 1e-6
        self.sot.setSize(self.robot.dimension)

        # Set second order computation
        self.robot.device.setSecondOrderIntegration()
        self.sot.setSecondOrderKinematics()
        plug(self.robot.device.velocity, self.robot.dynamic.velocity)
        plug(self.robot.dynamic.velocity, self.sot.velocity)
        """
        # Plug the solver control into the filter.
        plug(self.sot.control, self.jointLimitator.controlIN)

        # Important: always use 'jointLimitator.control'
        # and NOT 'sot.control'!

        if robot.device:
            plug(self.jointLimitator.control, robot.device.control)
        
        """

        # Plug the solver control into the robot.
        plug(self.sot.control, robot.device.control)

    def push(self, task):
        """
        Proxy method to push a task (not a MetaTask) in the sot
        """
        self.sot.push(task.name)
        if task.name != "taskposture" and "taskposture" in self.toList():
            self.sot.down("taskposture")

    def rm(self, task):
        """
        Proxy method to remove a task from the sot
        """
        self.sot.rm(task.name)

    def pop(self):
        """
        Proxy method to remove the last (usually posture) task from the sot
        """
        self.sot.pop()

    def __str__(self):
        return self.sot.display()

    def toList(self):
        """
        Creates the list of the tasks in the sot
        """
        return map(lambda x: x[1:-1], self.sot.dispStack().split('|')[1:])

    def clear(self):
        """
        Proxy method to remove all tasks from the sot
        """
        self.sot.clear()
Ejemplo n.º 12
0
    def buildurrobot(self):
        self.robot = Ur("ur5", device=RobotSimu("ur5"))
        robot_dimension = self.robot.dynamic.getDimension()
        self.robot.device.resize(robot_dimension)

        # Ros binding
        # roscore must be running
        ros = Ros(self.robot)

        # collision components
        self.a = sc.SotCollision("sc")

        # self.a.createlinkmodel(((0,0.08,0.01,0,0,0.0,0.0,0,0),(1,0.08,0.04,0,0,0,0,0,0),(2,0.09,0.15,0.2,0.0,0.0,0,1.57,0),(3,0.07,0.14,0.2,0.0,0.0,-3.1416,1.5706,-3.1416),(4,0.05,0.03,0.0,0.093,0,-3.14,0,-3.14),(5,0.057,0.02,0.0,0,-0.095,-3.14,0,-3.14),(6,0.04,0.01,0.0,0.065,0,1.57,0,0)))

        # create links for collision check
        self.a.createcollisionlink("base_link", "capsule", "internal", (0.08, 0.01, 0, 0, 0, 0.0, 0.0, 0, 0))
        self.a.createcollisionlink("shoulder_pan_link", "capsule", "internal", (0.08, 0.04, 0, 0, 0, 0, 0, 0, 0))
        self.a.createcollisionlink(
            "shoulder_lift_link", "capsule", "internal", (0.09, 0.15, 0, 0.2, 0.0, 0.0, 0, 1.57, 0)
        )
        self.a.createcollisionlink(
            "elbow_link", "capsule", "internal", (0.07, 0.14, 0, 0.2, 0.0, 0.0, -3.1416, 1.5706, -3.1416)
        )
        self.a.createcollisionlink(
            "wrist_1_link", "capsule", "internal", (0.05, 0.03, 0, 0.0, 0.093, 0, -3.14, 0, -3.14)
        )
        self.a.createcollisionlink(
            "wrist_2_link", "capsule", "internal", (0.057, 0.02, 0, 0.0, 0, -0.095, -3.14, 0, -3.14)
        )
        self.a.createcollisionlink("wrist_3_link", "capsule", "internal", (0.04, 0.01, 0, 0.0, 0.065, 0, 1.57, 0, 0))

        # plug joint position and joint jacobian to collision checker entity
        plug(self.robot.dynamic.base_joint, self.a.base_link)
        plug(self.robot.dynamic.shoulder_pan_joint, self.a.shoulder_pan_link)
        plug(self.robot.dynamic.shoulder_lift_joint, self.a.shoulder_lift_link)
        plug(self.robot.dynamic.elbow_joint, self.a.elbow_link)
        plug(self.robot.dynamic.wrist_1_joint, self.a.wrist_1_link)
        plug(self.robot.dynamic.wrist_2_joint, self.a.wrist_2_link)
        plug(self.robot.dynamic.wrist_3_joint, self.a.wrist_3_link)

        plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link)
        plug(self.robot.dynamic.Jshoulder_pan_joint, self.a.Jshoulder_pan_link)
        plug(self.robot.dynamic.Jshoulder_lift_joint, self.a.Jshoulder_lift_link)
        plug(self.robot.dynamic.Jelbow_joint, self.a.Jelbow_link)
        plug(self.robot.dynamic.Jwrist_1_joint, self.a.Jwrist_1_link)
        plug(self.robot.dynamic.Jwrist_2_joint, self.a.Jwrist_2_link)
        plug(self.robot.dynamic.Jwrist_3_joint, self.a.Jwrist_3_link)

        # create collision pairs
        # base link
        self.a.createcollisionpair("base_link", "shoulder_lift_link")
        self.a.createcollisionpair("base_link", "elbow_link")
        self.a.createcollisionpair("base_link", "wrist_1_link")
        self.a.createcollisionpair("base_link", "wrist_2_link")
        self.a.createcollisionpair("base_link", "wrist_3_link")
        # shoulder pan link
        self.a.createcollisionpair("shoulder_pan_link", "wrist_1_link")
        self.a.createcollisionpair("shoulder_pan_link", "wrist_2_link")
        self.a.createcollisionpair("shoulder_pan_link", "wrist_3_link")
        self.a.createcollisionpair("shoulder_pan_link", "elbow_link")
        # shoulder lift link
        self.a.createcollisionpair("shoulder_lift_link", "wrist_1_link")
        self.a.createcollisionpair("shoulder_lift_link", "wrist_2_link")
        self.a.createcollisionpair("shoulder_lift_link", "wrist_3_link")
        # shoulder elbow link
        self.a.createcollisionpair("elbow_link", "wrist_2_link")
        self.a.createcollisionpair("elbow_link", "wrist_3_link")
        # shoulder wrist1 link
        self.a.createcollisionpair("wrist_1_link", "wrist_3_link")

        ######### task description for old type of solver############
        # Create task for the waist
        robot_pose = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
        feature_base = FeaturePosition(
            "position_base", self.robot.dynamic.base_joint, self.robot.dynamic.Jbase_joint, robot_pose
        )
        task_base = Task("waist_task")
        task_base.controlGain.value = 100.0
        task_base.add(feature_base.name)

        # Create task for the wrist3
        I4 = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
        feature_wrist = FeaturePosition(
            "position_wrist", self.robot.dynamic.wrist_3_joint, self.robot.dynamic.Jwrist_3_joint, I4
        )
        task_wrist = Task("wrist_task")
        task_wrist.controlGain.value = 1
        task_wrist.add(feature_wrist.name)

        ######### task description for new type of solver############
        # waist position task
        goal = ((1.0, 0, 0, 0), (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(100)
        # wrist_3 task
        goal = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
        self.task_wrist_metakine = MetaTaskKine6d(
            "task_wrist_metakine", self.robot.dynamic, "wrist_3_joint", "wrist_3_joint"
        )
        # task_wrist_metakine.opmodif = goal
        # task_wrist_metakine.feature.frame('desired')
        self.task_wrist_metakine.featureDes.position.value = goal
        self.task_wrist_metakine.gain.setConstant(1)

        ######### task description for collision task   ############

        self.task_collision_avoidance = TaskInequality("taskcollision")
        self.collision_feature = FeatureGeneric("collisionfeature")
        plug(self.a.collisionJacobian, self.collision_feature.jacobianIN)
        plug(self.a.collisionDistance, self.collision_feature.errorIN)

        # self.a.collisionDistance

        self.task_collision_avoidance.add(self.collision_feature.name)
        self.task_collision_avoidance.referenceInf.value = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0)  # min
        self.task_collision_avoidance.referenceSup.value = (
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
        )  # max
        self.task_collision_avoidance.dt.value = self.dt
        self.task_collision_avoidance.controlGain.value = 0.01

        # task.featureDes.xy.value = (0,0)
        """
        #task_wrist_metakine.opmodif = goal
        #task_wrist_metakine.feature.frame('desired')
        task_wrist_metakine.featureDes.position.value = goal
        task_wrist_metakine.gain.setConstant(1) 
        """

        ######### solver###########################################################

        ##### solver old############
        """
        self.solver = SOT ('solver')
        self.solver.setSize (robot_dimension)  
        self.solver.push (task_base.name)
        self.solver.push (task_wrist.name)
        plug (self.solver.control, self.robot.device.control)
        """
        self.solver = SolverKine("sot")
        self.solver.setSize(robot_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)
Ejemplo n.º 13
0
class RobotCollisionModel:
    def __init__(self):
        # Marker array for link collision model
        self.linkcollisionmodel = MarkerArray()
        # Define marker properties for link
        self.rcm_pub = rospy.Publisher("collision_model", MarkerArray)
        self.r = rospy.Rate(1)
        self.nolinks = 2
        self.dt = 0
        self.buildurrobot()

    def buildurrobot(self):
        # collision components

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

        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("base_link", "box", "internal", (0.65, 0.65, 0.25, 0.0, 0.0, 0.175, 0, 0, 0))
        self.a.createcollisionlink(
            "shoulder_right_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0)
        )
        self.a.createcollisionlink(
            "shoulder_left_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0)
        )

        self.a.createcollisionlink("upper_left_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0))
        self.a.createcollisionlink("upper_right_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0))

        self.a.createcollisionlink(
            "upper_left_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0)
        )
        self.a.createcollisionlink(
            "upper_right_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0)
        )
        self.a.createcollisionlink("upper_right_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 0.0, -0.0, 0, 0, 0))
        self.a.createcollisionlink("upper_left_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 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))

        # 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.platform1.value = ((1,0,0,12),(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))
        # print self.robot.dynamic
        plug(self.robot.dynamic.base_joint, self.a.base_link)
        plug(self.robot.dynamic.r_shoulder_pan_joint, self.a.shoulder_right_link)
        plug(self.robot.dynamic.l_shoulder_pan_joint, self.a.shoulder_left_link)

        plug(self.robot.dynamic.l_upper_arm_roll_joint, self.a.upper_left_arm)
        plug(self.robot.dynamic.r_upper_arm_roll_joint, self.a.upper_right_arm)

        plug(self.robot.dynamic.l_forearm_roll_joint, self.a.upper_left_forearm)
        plug(self.robot.dynamic.r_forearm_roll_joint, self.a.upper_right_forearm)

        plug(self.robot.dynamic.r_wrist_roll_joint, self.a.upper_right_wrist)
        plug(self.robot.dynamic.l_wrist_roll_joint, self.a.upper_left_wrist)

        # 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)

        # l_shoulder_pan_joint
        # l_shoulder_lift_joint
        # r_shoulder_pan_joint
        # r_shoulder_lift_joint

        # 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)
        plug(self.robot.dynamic.Jr_shoulder_pan_joint, self.a.Jshoulder_right_link)
        plug(self.robot.dynamic.Jl_shoulder_pan_joint, self.a.Jshoulder_left_link)

        plug(self.robot.dynamic.Jl_upper_arm_roll_joint, self.a.Jupper_left_arm)
        plug(self.robot.dynamic.Jr_upper_arm_roll_joint, self.a.Jupper_right_arm)

        plug(self.robot.dynamic.Jl_forearm_roll_joint, self.a.Jupper_left_forearm)
        plug(self.robot.dynamic.Jr_forearm_roll_joint, self.a.Jupper_right_forearm)

        plug(self.robot.dynamic.Jr_wrist_roll_joint, self.a.Jupper_right_wrist)
        plug(self.robot.dynamic.Jl_wrist_roll_joint, self.a.Jupper_left_wrist)

        # plug(self.robot.dynamic.Jbase_joint,self.a.Jbase_link_4)

        self.a.createcollisionpair("base_link", "upper_left_arm")
        self.a.createcollisionpair("base_link", "upper_right_arm")
        self.a.createcollisionpair("base_link", "upper_left_forearm")
        self.a.createcollisionpair("base_link", "upper_right_forearm")
        self.a.createcollisionpair("shoulder_left_link", "upper_left_forearm")
        self.a.createcollisionpair("shoulder_right_link", "upper_right_forearm")

        self.a.createcollisionpair("upper_left_forearm", "upper_right_forearm")
        self.a.createcollisionpair("upper_left_arm", "upper_right_arm")

        self.a.createcollisionpair("upper_left_arm", "upper_right_forearm")
        self.a.createcollisionpair("upper_left_forearm", "upper_right_arm")

        self.a.createcollisionpair("upper_left_wrist", "upper_right_wrist")
        self.a.createcollisionpair("upper_left_forearm", "upper_right_wrist")
        self.a.createcollisionpair("upper_left_arm", "upper_right_wrist")
        self.a.createcollisionpair("upper_right_wrist", "upper_left_wrist")
        self.a.createcollisionpair("upper_right_forearm", "upper_left_wrist")
        self.a.createcollisionpair("upper_right_arm", "upper_left_wrist")

        # joint limits
        self.robot.dynamic.upperJl.recompute(0)
        self.robot.dynamic.lowerJl.recompute(0)
        self.taskjl = TaskJointLimits("taskJL")
        plug(self.robot.dynamic.position, self.taskjl.position)
        self.taskjl.controlGain.value = 5
        self.taskjl.referenceInf.value = self.robot.dynamic.lowerJl.value
        # print self.robot.dynamic.lowerJl.value
        self.taskjl.referenceSup.value = self.robot.dynamic.upperJl.value
        # print self.robot.dynamic.upperJl.value
        self.taskjl.dt.value = 1

        # torsotask
        self.task_torso_metakine = MetaTaskKine6d(
            "task_torso_metakine", self.robot.dynamic, "torso_lift_joint", "torso_lift_joint"
        )
        goal_torso = ((1.0, 0, 0, -0.05), (0, 1.0, 0, -0.0), (0, 0, 1.0, 0.790675), (0, 0, 0, 1.0))
        self.task_torso_metakine.feature.frame("desired")
        # task_torso_metakine.feature.selec.value = '000100'#RzRyRxTzTyTx
        self.task_torso_metakine.gain.setConstant(10)
        self.task_torso_metakine.featureDes.position.value = goal_torso

        goal = ((1.0, 0, 0, 0.0), (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.task_l_wrist_metakine = MetaTaskKine6d(
            "task_l_wrist_metakine", self.robot.dynamic, "l_wrist_roll_joint", "l_wrist_roll_joint"
        )
        # self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),)
        self.goal_l_wrist = ((1.0, 0, 0, 0.649), (0, 1.0, 0, -0.042), (0, 0, 1.0, 0.845), (0, 0, 0, 1.0))
        # self.goal_l_wrist = ((1.,0,0,0.486),(0,1.,0,-0.251),(0,0,1.,0.826),(0,0,0,1.),)
        self.task_l_wrist_metakine.feature.frame("desired")
        self.task_l_wrist_metakine.feature.selec.value = "000111"  # RzRyRxTzTyTx
        self.task_l_wrist_metakine.gain.setConstant(6)
        self.task_l_wrist_metakine.featureDes.position.value = self.goal_l_wrist

        self.task_r_wrist_metakine = MetaTaskKine6d(
            "task_r_wrist_metakine", self.robot.dynamic, "r_wrist_roll_joint", "r_wrist_roll_joint"
        )
        # self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),)
        # self.goal_r_wrist = ((1.,0,0,0.455),(0,1.,0,-0.835),(0,0,1.,0.780),(0,0,0,1.),)
        self.goal_r_wrist = ((1.0, 0, 0, 0.590), (0, 1.0, 0, -0.188), (0, 0, 1.0, 1.107), (0, 0, 0, 1.0))
        self.task_r_wrist_metakine.feature.frame("desired")
        self.task_r_wrist_metakine.feature.selec.value = "000100"  # RzRyRxTzTyTx
        self.task_r_wrist_metakine.gain.setConstant(6)
        self.task_r_wrist_metakine.featureDes.position.value = self.goal_r_wrist

        self.task_collision_avoidance = TaskInequality("taskcollision")
        self.collision_feature = FeatureGeneric("collisionfeature")
        plug(self.a.collisionJacobian, self.collision_feature.jacobianIN)
        plug(self.a.collisionDistance, self.collision_feature.errorIN)
        self.task_collision_avoidance.add(self.collision_feature.name)
        self.task_collision_avoidance.referenceInf.value = (
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
        )
        self.task_collision_avoidance.referenceSup.value = (
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
        )
        self.task_collision_avoidance.dt.value = 1
        self.task_collision_avoidance.controlGain.value = 50.0

        self.solver = SolverKine("sot")
        self.solver.setSize(self.dimension)
        self.solver.damping.value = 3e-2
        self.solver.push(self.taskjl.name)
        self.solver.push(self.task_collision_avoidance.name)
        self.solver.push(self.task_waist_metakine.task.name)
        self.solver.push(self.task_torso_metakine.task.name)
        self.solver.push(self.task_r_wrist_metakine.task.name)
        self.solver.push(self.task_l_wrist_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)

    def publish(self):
        # self.robot.dynamic.Jwrist_2_joint.recompute(self.dt)
        # self.a.collisionDistance.recompute(self.dt)
        # print self.a.collisionDistance.value
        # self.a.collisionJacobian.recompute(self.dt)
        # print self.a.collisionDistance.value
        self.dt = self.dt + 1
        # self.r.sleep()
        # print self.robot.dynamic.base_joint.value
        tstart = dt.datetime.now()
        self.robot.device.increment(0.01)
        self.getstate()
        self.rcm_pub.publish(self.linkcollisionmodel)
        tend = dt.datetime.now()
        print(tend - tstart).microseconds
        self.robot.dynamic.l_upper_arm_roll_joint.recompute(self.dt)
        # print self.robot.dynamic.l_upper_arm_roll_joint.value

    def getstate(self):
        state = rcm.a.getfclstate()
        # print state
        # a = self.robot.dynamic.wrist_2_joint.value
        for i in range(9):
            self.link = Marker()
            self.link.type = Marker().CUBE
            # self.link.mesh_resource = "package://sot-collision/mesh/capsule/capsule.dae"
            self.link.header.frame_id = "/odom"
            self.link.header.stamp = rospy.Time.now()
            # self.link.ns = "basicshapes"
            self.link.id = i
            self.link.action = self.link.ADD
            # print state
            self.link.pose.position.x = state[i][0]
            self.link.pose.position.y = state[i][1]
            self.link.pose.position.z = state[i][2]
            self.link.pose.orientation.x = state[i][3]
            self.link.pose.orientation.y = state[i][4]
            self.link.pose.orientation.z = state[i][5]
            self.link.pose.orientation.w = state[i][6]
            self.link.scale.x = state[i][7]
            self.link.scale.y = state[i][8]
            self.link.scale.z = state[i][9]
            self.link.color.r = 0.0
            self.link.color.g = 1.0
            self.link.color.b = 0.0
            self.link.color.a = 1.0
            # pself.link.pose.orientation
            # self.link.lifetime =  rospy.Duration();
            self.linkcollisionmodel.markers.append(self.link)
Ejemplo n.º 14
0
def SotPr2(robot):
    SolverKine.toList = toList
    sot = SolverKine('sot')
    sot.setSize(robot.dimension)
    return sot
Ejemplo n.º 15
0
# Make sure signals are recomputed even if not used in the control graph

robot.traceDefaultSignals()








############## new solver###############


dimension = robot.dynamic.getDimension()
solver = SolverKine('sot')
solver.setSize (dimension) 



# create links for collision check
from dynamic_graph.sot.dyninv import SolverKine
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import TaskInequality, TaskJointLimits
import dynamic_graph.sotcollision as sc
#solver.damping.value =3e-2
a = sc.SotCollision("sc")
solver.damping.value =3
a.createcollisionlink("wall1","box","external",(2.04,0.015,0.3,0,0,0.0,0.0,0,0))
a.createcollisionlink("wall2","box","external",(0.015,-2.44,0.3,0,0,0,0,0,0))
a.createcollisionlink("wall3","box","external",(2.04,0.015,0.3,0,0,0.0,0.0,0,0))
Ejemplo n.º 16
0
robot.control.unplug()

# --- PG ---------------------------------------------------------
# --- PG ---------------------------------------------------------
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(dyn)
pg.plugZmp(robot)


# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------


# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.17)
taskRH.opmodif = matrixToTuple(handMgrip)

# --- STATIC COM (if not walking)
taskCom0 = MetaTaskKineCom(dyn)
Ejemplo n.º 17
0
robot.control.unplug()

# --- PG ---------------------------------------------------------
# --- PG ---------------------------------------------------------
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(dyn)
pg.plugZmp(robot)

# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control, robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------

# ---- TASK GRIP ---
taskRH = MetaTaskKine6d('rh', dyn, 'rh', 'right-wrist')
handMgrip = eye(4)
handMgrip[0:3, 3] = (0, 0, -0.17)
taskRH.opmodif = matrixToTuple(handMgrip)

# --- STATIC COM (if not walking)
taskCom0 = MetaTaskKineCom(dyn)
Ejemplo n.º 18
0
# --- PG ---------------------------------------------------------
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(dyn)
pg.plugZmp(robot)


# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------


# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')

taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')

robot.control.unplug()


# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------


# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0.07,-0.02,0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')

taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
Ejemplo n.º 20
0
class RobotCollisionModel:
    def __init__(self):
        # Marker array for link collision model
        self.linkcollisionmodel = MarkerArray()
        # Define marker properties for link

        self.rcm_pub = rospy.Publisher("collision_model", MarkerArray)
        self.r = rospy.Rate(100)
        self.nolinks = 6
        self.dt = 0
        self.buildurrobot()

    def buildurrobot(self):
        self.robot = Ur("ur5", device=RobotSimu("ur5"))
        robot_dimension = self.robot.dynamic.getDimension()
        self.robot.device.resize(robot_dimension)

        # Ros binding
        # roscore must be running
        ros = Ros(self.robot)

        # collision components
        self.a = sc.SotCollision("sc")

        # self.a.createlinkmodel(((0,0.08,0.01,0,0,0.0,0.0,0,0),(1,0.08,0.04,0,0,0,0,0,0),(2,0.09,0.15,0.2,0.0,0.0,0,1.57,0),(3,0.07,0.14,0.2,0.0,0.0,-3.1416,1.5706,-3.1416),(4,0.05,0.03,0.0,0.093,0,-3.14,0,-3.14),(5,0.057,0.02,0.0,0,-0.095,-3.14,0,-3.14),(6,0.04,0.01,0.0,0.065,0,1.57,0,0)))

        # create links for collision check
        self.a.createcollisionlink("base_link", "capsule", "internal", (0.08, 0.01, 0, 0, 0, 0.0, 0.0, 0, 0))
        self.a.createcollisionlink("shoulder_pan_link", "capsule", "internal", (0.08, 0.04, 0, 0, 0, 0, 0, 0, 0))
        self.a.createcollisionlink(
            "shoulder_lift_link", "capsule", "internal", (0.09, 0.15, 0, 0.2, 0.0, 0.0, 0, 1.57, 0)
        )
        self.a.createcollisionlink(
            "elbow_link", "capsule", "internal", (0.07, 0.14, 0, 0.2, 0.0, 0.0, -3.1416, 1.5706, -3.1416)
        )
        self.a.createcollisionlink(
            "wrist_1_link", "capsule", "internal", (0.05, 0.03, 0, 0.0, 0.093, 0, -3.14, 0, -3.14)
        )
        self.a.createcollisionlink(
            "wrist_2_link", "capsule", "internal", (0.057, 0.02, 0, 0.0, 0, -0.095, -3.14, 0, -3.14)
        )
        self.a.createcollisionlink("wrist_3_link", "capsule", "internal", (0.04, 0.01, 0, 0.0, 0.065, 0, 1.57, 0, 0))

        # plug joint position and joint jacobian to collision checker entity
        plug(self.robot.dynamic.base_joint, self.a.base_link)
        plug(self.robot.dynamic.shoulder_pan_joint, self.a.shoulder_pan_link)
        plug(self.robot.dynamic.shoulder_lift_joint, self.a.shoulder_lift_link)
        plug(self.robot.dynamic.elbow_joint, self.a.elbow_link)
        plug(self.robot.dynamic.wrist_1_joint, self.a.wrist_1_link)
        plug(self.robot.dynamic.wrist_2_joint, self.a.wrist_2_link)
        plug(self.robot.dynamic.wrist_3_joint, self.a.wrist_3_link)

        plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link)
        plug(self.robot.dynamic.Jshoulder_pan_joint, self.a.Jshoulder_pan_link)
        plug(self.robot.dynamic.Jshoulder_lift_joint, self.a.Jshoulder_lift_link)
        plug(self.robot.dynamic.Jelbow_joint, self.a.Jelbow_link)
        plug(self.robot.dynamic.Jwrist_1_joint, self.a.Jwrist_1_link)
        plug(self.robot.dynamic.Jwrist_2_joint, self.a.Jwrist_2_link)
        plug(self.robot.dynamic.Jwrist_3_joint, self.a.Jwrist_3_link)

        # create collision pairs
        # base link
        self.a.createcollisionpair("base_link", "shoulder_lift_link")
        self.a.createcollisionpair("base_link", "elbow_link")
        self.a.createcollisionpair("base_link", "wrist_1_link")
        self.a.createcollisionpair("base_link", "wrist_2_link")
        self.a.createcollisionpair("base_link", "wrist_3_link")
        # shoulder pan link
        self.a.createcollisionpair("shoulder_pan_link", "wrist_1_link")
        self.a.createcollisionpair("shoulder_pan_link", "wrist_2_link")
        self.a.createcollisionpair("shoulder_pan_link", "wrist_3_link")
        self.a.createcollisionpair("shoulder_pan_link", "elbow_link")
        # shoulder lift link
        self.a.createcollisionpair("shoulder_lift_link", "wrist_1_link")
        self.a.createcollisionpair("shoulder_lift_link", "wrist_2_link")
        self.a.createcollisionpair("shoulder_lift_link", "wrist_3_link")
        # shoulder elbow link
        self.a.createcollisionpair("elbow_link", "wrist_2_link")
        self.a.createcollisionpair("elbow_link", "wrist_3_link")
        # shoulder wrist1 link
        self.a.createcollisionpair("wrist_1_link", "wrist_3_link")

        ######### task description for old type of solver############
        # Create task for the waist
        robot_pose = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
        feature_base = FeaturePosition(
            "position_base", self.robot.dynamic.base_joint, self.robot.dynamic.Jbase_joint, robot_pose
        )
        task_base = Task("waist_task")
        task_base.controlGain.value = 100.0
        task_base.add(feature_base.name)

        # Create task for the wrist3
        I4 = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
        feature_wrist = FeaturePosition(
            "position_wrist", self.robot.dynamic.wrist_3_joint, self.robot.dynamic.Jwrist_3_joint, I4
        )
        task_wrist = Task("wrist_task")
        task_wrist.controlGain.value = 1
        task_wrist.add(feature_wrist.name)

        ######### task description for new type of solver############
        # waist position task
        goal = ((1.0, 0, 0, 0), (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(100)
        # wrist_3 task
        goal = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
        self.task_wrist_metakine = MetaTaskKine6d(
            "task_wrist_metakine", self.robot.dynamic, "wrist_3_joint", "wrist_3_joint"
        )
        # task_wrist_metakine.opmodif = goal
        # task_wrist_metakine.feature.frame('desired')
        self.task_wrist_metakine.featureDes.position.value = goal
        self.task_wrist_metakine.gain.setConstant(1)

        ######### task description for collision task   ############

        self.task_collision_avoidance = TaskInequality("taskcollision")
        self.collision_feature = FeatureGeneric("collisionfeature")
        plug(self.a.collisionJacobian, self.collision_feature.jacobianIN)
        plug(self.a.collisionDistance, self.collision_feature.errorIN)

        # self.a.collisionDistance

        self.task_collision_avoidance.add(self.collision_feature.name)
        self.task_collision_avoidance.referenceInf.value = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0)  # min
        self.task_collision_avoidance.referenceSup.value = (
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
        )  # max
        self.task_collision_avoidance.dt.value = self.dt
        self.task_collision_avoidance.controlGain.value = 0.01

        # task.featureDes.xy.value = (0,0)
        """
        #task_wrist_metakine.opmodif = goal
        #task_wrist_metakine.feature.frame('desired')
        task_wrist_metakine.featureDes.position.value = goal
        task_wrist_metakine.gain.setConstant(1) 
        """

        ######### solver###########################################################

        ##### solver old############
        """
        self.solver = SOT ('solver')
        self.solver.setSize (robot_dimension)  
        self.solver.push (task_base.name)
        self.solver.push (task_wrist.name)
        plug (self.solver.control, self.robot.device.control)
        """
        self.solver = SolverKine("sot")
        self.solver.setSize(robot_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)

    def publish(self):

        self.a.collisionJacobian.recompute(self.dt)
        self.a.collisionDistance.recompute(self.dt)
        print "colljac"
        print self.a.collisionJacobian.value
        print "colldist"
        print self.a.collisionDistance.value
        self.robot.device.increment(0.01)

        """     
        rcm.robot.device.set((8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,8E-58,,8E-58,8E-58))
        rcm.robot.device.increment(0.01)
        rcm.robot.dynamic.shoulder_pan_joint.recompute(dt)
        rcm.robot.dynamic.shoulder_lift_joint.recompute(dt)
        rcm.robot.dynamic.elbow_joint.recompute(dt)
        rcm.robot.dynamic.wrist_1_joint.recompute(dt)
        rcm.robot.dynamic.wrist_2_joint.recompute(dt)
        rcm.robot.dynamic.wrist_3_joint.recompute(dt)
        rcm.robot.dynamic.Jshoulder_pan_joint.recompute(dt)
        rcm.robot.dynamic.Jshoulder_lift_joint.recompute(dt)
        rcm.robot.dynamic.Jelbow_joint.recompute(dt)
        rcm.robot.dynamic.Jwrist_1_joint.recompute(dt)
        rcm.robot.dynamic.Jwrist_2_joint.recompute(dt)
        rcm.a.collisionJacobian.recompute(dt)
        print rcm.a.collisionJpair1.value
        dt = dt +1
        """
        self.robot.dynamic.shoulder_pan_joint.recompute(self.dt)
        self.robot.dynamic.shoulder_lift_joint.recompute(self.dt)
        self.robot.dynamic.elbow_joint.recompute(self.dt)
        self.robot.dynamic.wrist_1_joint.recompute(self.dt)
        self.robot.dynamic.wrist_2_joint.recompute(self.dt)
        self.robot.dynamic.wrist_3_joint.recompute(self.dt)
        self.robot.dynamic.Jshoulder_pan_joint.recompute(self.dt)
        self.robot.dynamic.Jshoulder_lift_joint.recompute(self.dt)
        self.robot.dynamic.Jelbow_joint.recompute(self.dt)
        self.robot.dynamic.Jwrist_1_joint.recompute(self.dt)
        self.robot.dynamic.Jwrist_2_joint.recompute(self.dt)
        # recompute pair
        # self.a.collisionJacobian.recompute(self.dt)
        # self.a.imdpair1.recompute(self.dt)
        # print self.a.imdpair1.value
        # print 'im here'
        self.dt = self.dt + 1
        # self.a.updatefclmodel(1)

        self.getstate()
        # print 'wrist_3'
        # print self.robot.dynamic.wrist_3_joint.value
        self.rcm_pub.publish(self.linkcollisionmodel)
        self.r.sleep()
        # self.linkcollisionmodel = MarkerArray()

    # 0.07 0.07 0.05
    #

    def getstate(self):
        state = rcm.a.getfclstate()
        # print state
        # a = self.robot.dynamic.wrist_2_joint.value
        for i in range(7):
            self.link = Marker()
            self.link.type = self.link.MESH_RESOURCE  # Cylinder
            self.link.mesh_resource = "package://sot-collision/mesh/capsule/capsule.dae"
            self.link.header.frame_id = "/odom"
            self.link.header.stamp = rospy.Time.now()
            # self.link.ns = "basicshapes"
            self.link.id = i
            self.link.action = self.link.ADD
            # print state
            self.link.pose.position.x = state[i][0]
            self.link.pose.position.y = state[i][1]
            self.link.pose.position.z = state[i][2]
            self.link.pose.orientation.x = state[i][3]
            self.link.pose.orientation.y = state[i][4]
            self.link.pose.orientation.z = state[i][5]
            self.link.pose.orientation.w = state[i][6]
            self.link.scale.x = state[i][7]
            self.link.scale.y = state[i][7]
            self.link.scale.z = state[i][8]
            self.link.color.r = 0.0
            self.link.color.g = 1.0
            self.link.color.b = 0.0
            self.link.color.a = 1.0
            # pself.link.pose.orientation
            # self.link.lifetime =  rospy.Duration();
            self.linkcollisionmodel.markers.append(self.link)
Ejemplo n.º 21
0
from dynamic_graph.sot.ur.robot import Ur
from dynamic_graph.sot.dyninv import SolverKine
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import TaskJointLimits
#from dynamic_graph.ros.ros_sot_robot_model import Ros

robot = Ur('Ur',device=RobotSimu('ur'))
plug(robot.device.state, robot.dynamic.position)
#ros = Ros(robot)

def toList(sot):
    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
    
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)

robot.dynamic.velocity.value = robot.dimension*(0.,)
robot.dynamic.acceleration.value = robot.dimension*(0.,)
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
robot.device.control.unplug()
plug(sot.control,robot.device.control)

from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
dt=1e-3
@loopInThread
Ejemplo n.º 22
0
plug(featureCom.errordot,taskComPD.errorDot)
taskComPD.controlGain.value = 40
taskComPD.setBeta(-1)

# --- CONSTRAINT SUPPORT FOOT
featureSupport = FeatureGeneric('featureSupport')
featureSupport.errorIN.value = 6*(0,)
plug(footSelection.Jfoot,featureSupport.jacobianIN)
constraintSupport = Task('constraintSupport')
constraintSupport.add(featureSupport.name)
constraintSupport.task.value = 6*(0,)

# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(constraintSupport.name)
sot.push(taskTwoFeet.name)
sot.push(taskWaist.task.name)
sot.push(taskComPD.name)

plug(sot.control,robot.control)

# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
Ejemplo n.º 23
0
taskComPD.setBeta(-1)

# --- TASK RIGHT FOOT
# Task right hand
taskRF = MetaTask6d('rf', dyn, 'rf', 'right-ankle')
taskLF = MetaTask6d('lf', dyn, 'lf', 'left-ankle')

plug(pg.rightfootref, taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
plug(pg.leftfootref, taskLF.featureDes.position)
taskLF.task.controlGain.value = 5

# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskComPD.name)

plug(sot.control, robot.control)

# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
Ejemplo n.º 24
0
plug(featureCom.errordot, taskComPD.errorDot)
taskComPD.controlGain.value = 40
taskComPD.setBeta(-1)

# --- CONSTRAINT SUPPORT FOOT
featureSupport = FeatureGeneric('featureSupport')
featureSupport.errorIN.value = 6 * (0, )
plug(footSelection.Jfoot, featureSupport.jacobianIN)
constraintSupport = Task('constraintSupport')
constraintSupport.add(featureSupport.name)
constraintSupport.task.value = 6 * (0, )

# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(constraintSupport.name)
sot.push(taskTwoFeet.name)
sot.push(taskWaist.task.name)
sot.push(taskComPD.name)

plug(sot.control, robot.control)

# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
Ejemplo n.º 25
0
    def buildurrobot(self):
        # collision components

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

        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("base_link", "box", "internal", (0.65, 0.65, 0.25, 0.0, 0.0, 0.175, 0, 0, 0))
        self.a.createcollisionlink(
            "shoulder_right_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0)
        )
        self.a.createcollisionlink(
            "shoulder_left_link", "box", "internal", (0.20, 0.23, 0.65, 0.0, 0.0, -0.13, 0, 0, 0)
        )

        self.a.createcollisionlink("upper_left_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0))
        self.a.createcollisionlink("upper_right_arm", "box", "internal", (0.42, 0.13, 0.15, 0.26, 0.0, -0.0, 0, 0, 0))

        self.a.createcollisionlink(
            "upper_left_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0)
        )
        self.a.createcollisionlink(
            "upper_right_forearm", "box", "internal", (0.37, 0.09, 0.09, 0.28, 0.0, -0.0, 0, 0, 0)
        )
        self.a.createcollisionlink("upper_right_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 0.0, -0.0, 0, 0, 0))
        self.a.createcollisionlink("upper_left_wrist", "box", "internal", (0.1, 0.09, 0.05, 0.1, 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))

        # 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.platform1.value = ((1,0,0,12),(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))
        # print self.robot.dynamic
        plug(self.robot.dynamic.base_joint, self.a.base_link)
        plug(self.robot.dynamic.r_shoulder_pan_joint, self.a.shoulder_right_link)
        plug(self.robot.dynamic.l_shoulder_pan_joint, self.a.shoulder_left_link)

        plug(self.robot.dynamic.l_upper_arm_roll_joint, self.a.upper_left_arm)
        plug(self.robot.dynamic.r_upper_arm_roll_joint, self.a.upper_right_arm)

        plug(self.robot.dynamic.l_forearm_roll_joint, self.a.upper_left_forearm)
        plug(self.robot.dynamic.r_forearm_roll_joint, self.a.upper_right_forearm)

        plug(self.robot.dynamic.r_wrist_roll_joint, self.a.upper_right_wrist)
        plug(self.robot.dynamic.l_wrist_roll_joint, self.a.upper_left_wrist)

        # 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)

        # l_shoulder_pan_joint
        # l_shoulder_lift_joint
        # r_shoulder_pan_joint
        # r_shoulder_lift_joint

        # 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)
        plug(self.robot.dynamic.Jr_shoulder_pan_joint, self.a.Jshoulder_right_link)
        plug(self.robot.dynamic.Jl_shoulder_pan_joint, self.a.Jshoulder_left_link)

        plug(self.robot.dynamic.Jl_upper_arm_roll_joint, self.a.Jupper_left_arm)
        plug(self.robot.dynamic.Jr_upper_arm_roll_joint, self.a.Jupper_right_arm)

        plug(self.robot.dynamic.Jl_forearm_roll_joint, self.a.Jupper_left_forearm)
        plug(self.robot.dynamic.Jr_forearm_roll_joint, self.a.Jupper_right_forearm)

        plug(self.robot.dynamic.Jr_wrist_roll_joint, self.a.Jupper_right_wrist)
        plug(self.robot.dynamic.Jl_wrist_roll_joint, self.a.Jupper_left_wrist)

        # plug(self.robot.dynamic.Jbase_joint,self.a.Jbase_link_4)

        self.a.createcollisionpair("base_link", "upper_left_arm")
        self.a.createcollisionpair("base_link", "upper_right_arm")
        self.a.createcollisionpair("base_link", "upper_left_forearm")
        self.a.createcollisionpair("base_link", "upper_right_forearm")
        self.a.createcollisionpair("shoulder_left_link", "upper_left_forearm")
        self.a.createcollisionpair("shoulder_right_link", "upper_right_forearm")

        self.a.createcollisionpair("upper_left_forearm", "upper_right_forearm")
        self.a.createcollisionpair("upper_left_arm", "upper_right_arm")

        self.a.createcollisionpair("upper_left_arm", "upper_right_forearm")
        self.a.createcollisionpair("upper_left_forearm", "upper_right_arm")

        self.a.createcollisionpair("upper_left_wrist", "upper_right_wrist")
        self.a.createcollisionpair("upper_left_forearm", "upper_right_wrist")
        self.a.createcollisionpair("upper_left_arm", "upper_right_wrist")
        self.a.createcollisionpair("upper_right_wrist", "upper_left_wrist")
        self.a.createcollisionpair("upper_right_forearm", "upper_left_wrist")
        self.a.createcollisionpair("upper_right_arm", "upper_left_wrist")

        # joint limits
        self.robot.dynamic.upperJl.recompute(0)
        self.robot.dynamic.lowerJl.recompute(0)
        self.taskjl = TaskJointLimits("taskJL")
        plug(self.robot.dynamic.position, self.taskjl.position)
        self.taskjl.controlGain.value = 5
        self.taskjl.referenceInf.value = self.robot.dynamic.lowerJl.value
        # print self.robot.dynamic.lowerJl.value
        self.taskjl.referenceSup.value = self.robot.dynamic.upperJl.value
        # print self.robot.dynamic.upperJl.value
        self.taskjl.dt.value = 1

        # torsotask
        self.task_torso_metakine = MetaTaskKine6d(
            "task_torso_metakine", self.robot.dynamic, "torso_lift_joint", "torso_lift_joint"
        )
        goal_torso = ((1.0, 0, 0, -0.05), (0, 1.0, 0, -0.0), (0, 0, 1.0, 0.790675), (0, 0, 0, 1.0))
        self.task_torso_metakine.feature.frame("desired")
        # task_torso_metakine.feature.selec.value = '000100'#RzRyRxTzTyTx
        self.task_torso_metakine.gain.setConstant(10)
        self.task_torso_metakine.featureDes.position.value = goal_torso

        goal = ((1.0, 0, 0, 0.0), (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.task_l_wrist_metakine = MetaTaskKine6d(
            "task_l_wrist_metakine", self.robot.dynamic, "l_wrist_roll_joint", "l_wrist_roll_joint"
        )
        # self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),)
        self.goal_l_wrist = ((1.0, 0, 0, 0.649), (0, 1.0, 0, -0.042), (0, 0, 1.0, 0.845), (0, 0, 0, 1.0))
        # self.goal_l_wrist = ((1.,0,0,0.486),(0,1.,0,-0.251),(0,0,1.,0.826),(0,0,0,1.),)
        self.task_l_wrist_metakine.feature.frame("desired")
        self.task_l_wrist_metakine.feature.selec.value = "000111"  # RzRyRxTzTyTx
        self.task_l_wrist_metakine.gain.setConstant(6)
        self.task_l_wrist_metakine.featureDes.position.value = self.goal_l_wrist

        self.task_r_wrist_metakine = MetaTaskKine6d(
            "task_r_wrist_metakine", self.robot.dynamic, "r_wrist_roll_joint", "r_wrist_roll_joint"
        )
        # self.goal_l_wrist = ((1.,0,0,0.748),(0,1.,0,-0.246),(0,0,1.,0.639),(0,0,0,1.),)
        # self.goal_r_wrist = ((1.,0,0,0.455),(0,1.,0,-0.835),(0,0,1.,0.780),(0,0,0,1.),)
        self.goal_r_wrist = ((1.0, 0, 0, 0.590), (0, 1.0, 0, -0.188), (0, 0, 1.0, 1.107), (0, 0, 0, 1.0))
        self.task_r_wrist_metakine.feature.frame("desired")
        self.task_r_wrist_metakine.feature.selec.value = "000100"  # RzRyRxTzTyTx
        self.task_r_wrist_metakine.gain.setConstant(6)
        self.task_r_wrist_metakine.featureDes.position.value = self.goal_r_wrist

        self.task_collision_avoidance = TaskInequality("taskcollision")
        self.collision_feature = FeatureGeneric("collisionfeature")
        plug(self.a.collisionJacobian, self.collision_feature.jacobianIN)
        plug(self.a.collisionDistance, self.collision_feature.errorIN)
        self.task_collision_avoidance.add(self.collision_feature.name)
        self.task_collision_avoidance.referenceInf.value = (
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
            0.1,
        )
        self.task_collision_avoidance.referenceSup.value = (
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
            2e10,
        )
        self.task_collision_avoidance.dt.value = 1
        self.task_collision_avoidance.controlGain.value = 50.0

        self.solver = SolverKine("sot")
        self.solver.setSize(self.dimension)
        self.solver.damping.value = 3e-2
        self.solver.push(self.taskjl.name)
        self.solver.push(self.task_collision_avoidance.name)
        self.solver.push(self.task_waist_metakine.task.name)
        self.solver.push(self.task_torso_metakine.task.name)
        self.solver.push(self.task_r_wrist_metakine.task.name)
        self.solver.push(self.task_l_wrist_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)
Ejemplo n.º 26
0
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import TaskJointLimits
#from dynamic_graph.ros.ros_sot_robot_model import Ros

robot = Ur('Ur', device=RobotSimu('ur'))
plug(robot.device.state, robot.dynamic.position)
#ros = Ros(robot)


def toList(sot):
    return map(lambda x: x[1:-1], sot.dispStack().split('|')[1:])


SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)

robot.dynamic.velocity.value = robot.dimension * (0., )
robot.dynamic.acceleration.value = robot.dimension * (0., )
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics', 'true')
robot.dynamic.setProperty('ComputeAccelerationCoM', 'true')
robot.device.control.unplug()
plug(sot.control, robot.device.control)

from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
dt = 1e-3
Ejemplo n.º 27
0
robot.control.unplug()

# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine


def toList(sot):
    return map(lambda x: x[1:-1], sot.dispStack().split('|')[1:])


SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control, robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------

# ---- TASK GRIP ---
taskRH = MetaTaskKine6d('rh', dyn, 'rh', 'right-wrist')
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.07, -0.02, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')

taskLH = MetaTaskKine6d('lh', dyn, 'lh', 'left-wrist')
Ejemplo n.º 28
0
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value    = gearRatio[robotName]

plug(robot.state,dyn.position)
dyn.velocity.value = robotDim*(0.,)
dyn.acceleration.value = robotDim*(0.,)

# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(dyn)
pg.plugZmp(robot)

# ---- SOT ---------------------------------------------------------------------
# The basic SOT solver would work too.
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
taskWaist=MetaTask6d('waist',dyn,'waist','waist')
pg.plugWaistTask(taskWaist)
taskWaist.task.controlGain.value = 5
taskWaist.feature.selec.value = '011100'

# --- TASK COM ---
taskCom = MetaTaskKineCom(dyn,"compd")
pg.plugComTask(taskCom)
taskCom.feature.selec.value = '011'
Ejemplo n.º 29
0
Device = PyEntityFactoryClass('SotDevice')

robot = RobotBuilder(name='robot', device=Device('robot_device'))

print("Number of dofs (6 for the freeflyer + num of joints):")
print robot.dimension

# FIXME: this must be set so that the graph can be evaluated.
robot.device.zmp.value = (0., 0., 0.)

# Create a solver.
from dynamic_graph.sot.dyninv import SolverKine
def toList(solver):
    return map(lambda x: x[1:-1],solver.dispStack().split('|')[1:])
SolverKine.toList = toList
solver = SolverKine('sot')
solver.setSize(robot.dimension)
# set a constant damping to 0.1
solver.damping.value = 0.1

robot.device.control.unplug()
plug(solver.control,robot.device.control)
# Close the control loop
plug(robot.device.state,robot.dynamic.position)

print("...done!")

# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
Ejemplo n.º 30
0
#taskComPD.setBeta(-1)

# --- TASK RIGHT FOOT
# Task right hand
taskRF=MetaTask6d('rf',dyn,'rf','right-ankle')
taskLF=MetaTask6d('lf',dyn,'lf','left-ankle')

plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 5

# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskComPD.name)

plug(sot.control,robot.control)

# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
from dynamic_graph.tracer_real_time import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
Ejemplo n.º 31
0
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')

robot.control.unplug()


# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------

# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')

taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
Ejemplo n.º 32
0
print("Number of dofs (6 for the freeflyer + num of joints):")
print robot.dimension

# FIXME: this must be set so that the graph can be evaluated.
robot.device.zmp.value = (0., 0., 0.)

# Create a solver.
from dynamic_graph.sot.dyninv import SolverKine


def toList(solver):
    return map(lambda x: x[1:-1], solver.dispStack().split('|')[1:])


SolverKine.toList = toList
solver = SolverKine('sot')
solver.setSize(robot.dimension)
# set a constant damping to 0.1
solver.damping.value = 0.1

robot.device.control.unplug()
plug(solver.control, robot.device.control)
# Close the control loop
plug(robot.device.state, robot.dynamic.position)

print("...done!")

# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
Ejemplo n.º 33
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)
Ejemplo n.º 34
0
taskPosture = Task('taskPosture')
taskPosture.add('featurePosture')
plug(taskPosture.error, gainPosture.error)
plug(gainPosture.gain, taskPosture.controlGain)

# --- TASK RIGHT FOOT
# Task right hand
#plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
#plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 5

# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control, robot.control)

# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
from dynamic_graph.tracer_real_time import *
tr = Tracer('tr')
tr.open('/tmp/', '', '.dat')
tr.start()
robot.after.addSignal('tr.triger')

#tr.add(dyn.name+'.ffposition','ff')
tr.add(taskRF.featureDes.name + '.position', 'refr')
tr.add(taskLF.featureDes.name + '.position', 'refl')
tr.add('dyn.rf', 'r')
Ejemplo n.º 35
0
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()

robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')

# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)

# The control signal, previously plugged to the velocity (kinematic mode) is
#  redirected.
robot.device.control.unplug()
plug(sot.control,robot.device.control)

# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------


# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist')
# change the position of the operational point wrt the wrist
Ejemplo n.º 36
0
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]

plug(robot.state, dyn.position)
dyn.velocity.value = robotDim * (0., )
dyn.acceleration.value = robotDim * (0., )

# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(dyn)
pg.plugZmp(robot)

# ---- SOT ---------------------------------------------------------------------
# The basic SOT solver would work too.
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control, robot.control)

# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
taskWaist = MetaTask6d('waist', dyn, 'waist', 'waist')
pg.plugWaistTask(taskWaist)
taskWaist.task.controlGain.value = 5
taskWaist.feature.selec.value = '011100'

# --- TASK COM ---
taskCom = MetaTaskKineCom(dyn, "compd")
pg.plugComTask(taskCom)
taskCom.feature.selec.value = '011'