def test_pr2(self):
pr2_robot = robots.PR2()
env = Environment()
# env.SetViewer("qtcoin")
robot = env.ReadRobotXMLFile(pr2_robot.shape)
env.Add(robot)
dof_val = robot.GetActiveDOFValues()
init_dof = np.zeros((39,))
self.assertTrue(np.allclose(dof_val, init_dof, 1e-6))
def test_basic(self):
pr2_robot = pr2.PR2()
env = Environment()
robot = env.ReadRobotXMLFile(pr2_robot.shape)
env.Add(robot)
dof_val = robot.GetActiveDOFValues()
init_dof = np.array([
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.77555756e-17,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.77555756e-17,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00
])
self.assertTrue(np.allclose(dof_val, init_dof, 1e-6))
if len(point) == 3:
point = np.concatenate([point, np.array([1])])
elif len(point) != 4:
print 'Invalid dimension'
return
transformed_point = trans_dot( target_transform, point) # equation 2.23 in Murray
return transformed_point[:-1]
# define object and place it (0,0,0)
width = 0.07
length = 0.03
height = 0.1
objA = box_body(env,width,length,height,\
name='objA',\
color=(0, 1, 1))
env.Add(objA)
width = 3
length = 5
height = 4
objB = box_body(env,width,length,height,\
name='objB',\
color=(1, 1, 1))
env.Add(objB)
set_point(objB,[1,0,0])
posB = objB.ComputeAABB().pos()
extB = objB.ComputeAABB().extents()
k=1
vtx = box_body(env,0.1,0.1,0.1,name='vtx+%d'%(k),color=(1,0,0))
env.Add(vtx)
set_point(vtx,posB+extB)
import copy
env = Environment()
ENV_FILENAME = '../..//env/just_robot.xml'
env.SetViewer('qtcoin')
env.Load(ENV_FILENAME)
robot = env.GetRobots()[0]
# define object and place it (0,0,0)
width = 0.07
length = 0.03
height = 0.1
obj = box_body(env, width, length, height, \
name='obj', \
color=(0, 1, 1))
env.Add(obj)
# define robot
robot.SetActiveManipulator('leftarm')
manip = robot.GetManipulator('leftarm')
ee = manip.GetEndEffector()
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel.load():
ikmodel.autogenerate()
robot.SetDOFValues(np.array([0.54800022]),
robot.GetActiveManipulator().GetGripperIndices())