Exemplo n.º 1
0
  joint = "base_joint"
  handle = "handle"
# 4}}}

Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)

#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
vf.loadObjectModel (Box, "box1")
vf.loadObjectModel (Box, "box2")

robot.setJointBounds ('box1/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
robot.setJointBounds ('box2/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
# 3}}}

# Define configurations. {{{3
q_init = robot.getCurrentConfig ()

q0 = q_init [::]
rank = robot.rankInConfiguration ['box1/base_joint_xyz']
q_init [rank:rank+3] = [-0.3, 1.3, 0.76]
rank = robot.rankInConfiguration ['box1/base_joint_SO3']
c = sqrt (2) / 2
q_init [rank:rank+4] = [c, 0, c, 0]
rank = robot.rankInConfiguration ['box2/base_joint_xyz']
q_init [rank:rank+3] = [-0.3, 1.3, 0.76]
rank = robot.rankInConfiguration ['box2/base_joint_SO3']
  joint = "base_joint"
  handle = "handle"
# 4}}}

Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)

#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
vf.loadObjectModel (Box, "box1")
vf.loadObjectModel (Box, "box2")

robot.setJointBounds ('box1/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
robot.setJointBounds ('box2/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
# 3}}}

# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB1 = robot.rankInConfiguration ['box1/base_joint_xyz']
rankB2 = robot.rankInConfiguration ['box2/base_joint_xyz']

c = sqrt (2) / 2
rank = robot.rankInConfiguration ['box1/base_joint_xyz']
q_init [rank:rank+3] = [-0.3, 0.5, 0.76]
rank = robot.rankInConfiguration ['box1/base_joint_SO3']
q_init [rank:rank+4] = [c, 0, -c, 0]
rank = robot.rankInConfiguration ['box2/base_joint_xyz']
q_init [rank:rank+3] = [-0.4, 0.5, 0.76]
Exemplo n.º 3
0
  handle = "handle"
# 4}}}

Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)

#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 0, 0, 0, 1])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(K):
  boxes.append ("box" + str(i))
  vf.loadObjectModel (Box, boxes[i])
  robot.setJointBounds (boxes[i]+ '/root_joint', [-1,0.5,-1,2,0.6,1.9,-1,1,-1,1,-1,1,-1,1])

def setBoxColors (gui):
  c = Color()
  for i in xrange(K):
    gui.setColor (boxes[i], c[i])
# 3}}}

# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB = list()
for i in xrange(K):
  rankB.append (robot.rankInConfiguration [boxes[i] + '/root_joint'])

bb = [-0.3, -0.4, 0.7, 0.9]
c = sqrt (2) / 2
Exemplo n.º 4
0
# 4}}}

Robot.urdfSuffix = ""
robot = Robot('baxter-manip', 'baxter')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)

#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition("baxter", [-0.8, 0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel(Table, "table")
boxes = list()
for i in xrange(K):
    boxes.append("box" + str(i))
    vf.loadObjectModel(Box, boxes[i])
    robot.setJointBounds(boxes[i] + '/base_joint_xyz',
                         [-1, 0.5, -1, 2, 0.6, 1.9])


def setBoxColors(gui):
    c = Color()
    for i in xrange(K):
        gui.setColor(boxes[i], c[i])


# 3}}}

# Define configurations. {{{3
q_init = robot.getCurrentConfig()
rankB = list()
for i in xrange(K):
    rankB.append(robot.rankInConfiguration[boxes[i] + '/base_joint_xyz'])
  handle = "handle"
# 4}}}

Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)

#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(K):
  boxes.append ("box" + str(i))
  vf.loadObjectModel (Box, boxes[i])
  robot.setJointBounds (boxes[i]+ '/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])

def setBoxColors (gui):
  c = Color()
  for i in xrange(K):
    gui.setColor (boxes[i], c[i])
# 3}}}

# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB = list()
for i in xrange(K):
  rankB.append (robot.rankInConfiguration [boxes[i] + '/base_joint_xyz'])

bb = [-0.3, -0.4, 0.7, 0.9]
c = sqrt (2) / 2