<commit_before># python

# Import our planning packages
import aminopy as aa
import tmsmtpy as tm

import TMSMT

# Import python math package
from math import pi

# The Common Lisp runtime is also available
import CL as cl


## Some domain parameters

# Table grid resolution
RESOLUTION=0.1

# End-Effector Frame
FRAME = "right_endpoint"

# Object placement tolerance
EPSILON = 1e-4

# Nominal relative grasp pose
GRASP = aa.tf2( aa.yangle(pi),
                aa.vec3([0.00, 0.00, 0.075]))


def scene_state_linear(scene, configuration):
    '''Map the scene graph `scene' to a task state expression'''

    ## terms in the expression
    conjunction = []

    def handle_moveable(frame):
        name = frame['name']
        parent_name = frame['parent']
        position = parent_name

        try:
            # If parent frame is a placement surface, position is the
            # appropriate grid cell on the surface.
            parent_frame = scene[parent_name]
            if aa.frame_isa(parent_frame, "surface"):
                trans = aa.translation( aa.frame_fixed_tf(frame) )
                i = int(round( trans[0] / RESOLUTION))
                j = int(round( trans[1] / RESOLUTION))
                position = tm.mangle(parent_name,i,j)
        except NameError:
            pass

        conjunction.append(["=",
                            ["POSITION", name],
                            position])

    moveable_frames = tm.collect_frame_type(scene,"moveable")
    map(handle_moveable, moveable_frames)

    return conjunction

## END def scene_state_linear


def scene_objects_linear(scene):
    '''Return the PDDL objects for `scene'.'''
    obj = []

    def type_names(thing):
        return [ f['name']
                 for f in
                 tm.collect_frame_type(scene,thing) ]

    # Moveable objects are blocks
    moveable = type_names("moveable")
    moveable.insert(0, "BLOCK")

    # Draw grid on surfaces
    locations = ['LOCATION']
    def add_loc(name,i,j):
        locations.append(tm.mangle(name,i,j))

    for frame in tm.collect_frame_type(scene,"surface"):
        name = frame['name']
        for geom in frame['geometry']:
            shape = geom['shape']
            if aa.shape_is_box(shape):
                d = shape['dimension']
                x_max = d[0] / 2
                y_max = d[1] / 2
                x = 0
                i = 0
                while x <= x_max:
                    y = 0
                    j = 0
                    while y <= y_max:
                        add_loc(name,i,j)
                        if i > 0: add_loc(name,-i,j)
                        if j > 0: add_loc(name,i,-j)
                        if i > 0 and j > 0: add_loc(name,-i,-j)
                        y += RESOLUTION
                        j+=1
                    x +=  RESOLUTION
                    i+=1

    #print surface
    return [moveable, locations]

## END def scene_objects_linear(scene):


############################
### Operator Definitions ###
############################


def motion_plan(op,goal):
    scene = op['final_scene']
    ssg = aa.scene_chain(scene, "", FRAME)
    mp = tm.op_motion( op, ssg, goal )
    if False == mp: raise tm.PlanningFailure()
    return mp

def pick(op, obj):
    g_tf_o = tm.op_tf_abs(op,obj)
    g_tf_e = aa.mul(g_tf_o, GRASP)
    mp = motion_plan(op, g_tf_e)
    return tm.plan(mp, tm.op_reparent(mp, FRAME, obj))

def place_tf(op, obj, dst_frame, g_tf_o ):
    o_tf_e = tm.op_tf_rel(op,obj,FRAME)
    g_tf_e = aa.mul( g_tf_o, o_tf_e )
    mp =  motion_plan(op, g_tf_e)
    return tm.plan(mp, tm.op_reparent(mp, dst_frame, obj))

def place_height(scene,name):<commit_msg>Plan for object frame when placing<commit_after># python

# Import our planning packages
import aminopy as aa
import tmsmtpy as tm

import TMSMT

# Import python math package
from math import pi

# The Common Lisp runtime is also available
import CL as cl


## Some domain parameters

# Table grid resolution
RESOLUTION=0.1

# End-Effector Frame
FRAME = "right_endpoint"

# Object placement tolerance
EPSILON = 1e-4

# Nominal relative grasp pose
GRASP = aa.tf2( aa.yangle(pi),
                aa.vec3([0.00, 0.00, 0.075]))


def scene_state_linear(scene, configuration):
    '''Map the scene graph `scene' to a task state expression'''

    ## terms in the expression
    conjunction = []

    def handle_moveable(frame):
        name = frame['name']
        parent_name = frame['parent']
        position = parent_name

        try:
            # If parent frame is a placement surface, position is the
            # appropriate grid cell on the surface.
            parent_frame = scene[parent_name]
            if aa.frame_isa(parent_frame, "surface"):
                trans = aa.translation( aa.frame_fixed_tf(frame) )
                i = int(round( trans[0] / RESOLUTION))
                j = int(round( trans[1] / RESOLUTION))
                position = tm.mangle(parent_name,i,j)
        except NameError:
            pass

        conjunction.append(["=",
                            ["POSITION", name],
                            position])

    moveable_frames = tm.collect_frame_type(scene,"moveable")
    map(handle_moveable, moveable_frames)

    return conjunction

## END def scene_state_linear


def scene_objects_linear(scene):
    '''Return the PDDL objects for `scene'.'''
    obj = []

    def type_names(thing):
        return [ f['name']
                 for f in
                 tm.collect_frame_type(scene,thing) ]

    # Moveable objects are blocks
    moveable = type_names("moveable")
    moveable.insert(0, "BLOCK")

    # Draw grid on surfaces
    locations = ['LOCATION']
    def add_loc(name,i,j):
        locations.append(tm.mangle(name,i,j))

    for frame in tm.collect_frame_type(scene,"surface"):
        name = frame['name']
        for geom in frame['geometry']:
            shape = geom['shape']
            if aa.shape_is_box(shape):
                d = shape['dimension']
                x_max = d[0] / 2
                y_max = d[1] / 2
                x = 0
                i = 0
                while x <= x_max:
                    y = 0
                    j = 0
                    while y <= y_max:
                        add_loc(name,i,j)
                        if i > 0: add_loc(name,-i,j)
                        if j > 0: add_loc(name,i,-j)
                        if i > 0 and j > 0: add_loc(name,-i,-j)
                        y += RESOLUTION
                        j+=1
                    x +=  RESOLUTION
                    i+=1

    #print surface
    return [moveable, locations]

## END def scene_objects_linear(scene):


############################
### Operator Definitions ###
############################


def motion_plan(op,frame, goal):
    scene = op['final_scene']
    ssg = aa.scene_chain(scene, "", frame)
    mp = tm.op_motion( op, ssg, goal )
    if False == mp: raise tm.PlanningFailure()
    return mp

def pick(op, obj):
    g_tf_o = tm.op_tf_abs(op,obj)
    g_tf_e = aa.mul(g_tf_o, GRASP)
    mp = motion_plan(op, FRAME, g_tf_e)
    return tm.plan(mp, tm.op_reparent(mp, FRAME, obj))

def place_tf(op, obj, dst_frame, g_tf_o ):
    mp =  motion_plan(op, obj, g_tf_o)
    return tm.plan(mp, tm.op_reparent(mp, dst_frame, obj))

def place_height(scene,name):
{'fraction_non_alphanumeric': 0.12079803224924843, 'fraction_numerical': 0.009018857611369226, 'mean_word_length': 1.9393574297188756, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6248242', 'n_tokens_mistral': 2392, 'n_tokens_neox': 2252, 'n_words': 593}