def add_drop_table(env, dist_from_robot): thickness = 0.1 legheight = 0.4 table = object_models.create_table(env, "table", 0.5, 0.5, thickness, 0.1, 0.1, legheight) x, y = dist_from_robot z = thickness / 2 + legheight rot = openravepy.quatFromAxisAngle((0, 0, 0)) table.SetTransform(openravepy.matrixFromPose(list(rot) + [x, y, z])) env.AddKinBody(table) return table
def spawn_table_pr2(env, robot_dist_from_table, tabletype='rll'): if tabletype == 'rll': thickness = 0.2 legheight = 0.6 table = object_models.create_table(env, TABLE_NAMES[tabletype], 2.235, 0.94, thickness, 1.3, 0.6, legheight) elif tabletype == 'small': thickness = 0.2 legheight = 0.6 table = object_models.create_table(env, TABLE_NAMES[tabletype], 0.7 - robot_dist_from_table, 1.5, thickness, 0.2 - robot_dist_from_table, 0.2, legheight) x = -utils.get_object_limits(table)[0] + 0.35 + robot_dist_from_table y = 0 z = thickness / 2 + legheight table.SetTransform(openravepy.matrixFromPose([1, 0, 0, 0, x, y, z])) # env.AddKinBody(table) env.AddRobot(table) # robot = env.ReadRobotXMLFile("../models/pr2/pr2-head-kinect.xml") robot = env.ReadRobotXMLFile("../models/pr2/pr2.zae") env.Add(robot)