├── cio ├── __init__.py ├── params.py ├── optimization.py ├── world.py └── util.py ├── example ├── __init__.py └── minimal.py ├── notebook_files ├── L_ci_vis.gif ├── L_task_vis.gif ├── cis_cost.png ├── cone_cost.png ├── final_cost.png ├── phys_cost.png ├── task_cost.png ├── L_task_phys_vis.gif ├── contact_state.png ├── stage_0_weights.png └── stage_1_weights.png ├── .gitignore ├── setup.py ├── README.md └── notebook_tutorial.ipynb /cio/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /example/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /notebook_files/L_ci_vis.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/L_ci_vis.gif -------------------------------------------------------------------------------- /notebook_files/L_task_vis.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/L_task_vis.gif -------------------------------------------------------------------------------- /notebook_files/cis_cost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/cis_cost.png -------------------------------------------------------------------------------- /notebook_files/cone_cost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/cone_cost.png -------------------------------------------------------------------------------- /notebook_files/final_cost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/final_cost.png -------------------------------------------------------------------------------- /notebook_files/phys_cost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/phys_cost.png -------------------------------------------------------------------------------- /notebook_files/task_cost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/task_cost.png -------------------------------------------------------------------------------- /notebook_files/L_task_phys_vis.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/L_task_phys_vis.gif -------------------------------------------------------------------------------- /notebook_files/contact_state.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/contact_state.png -------------------------------------------------------------------------------- /notebook_files/stage_0_weights.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/stage_0_weights.png -------------------------------------------------------------------------------- /notebook_files/stage_1_weights.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/CIO/HEAD/notebook_files/stage_1_weights.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .ipynb_checkpoints/ 3 | .DS_Store 4 | __pycache__ 5 | example/output 6 | output/ 7 | CIO.egg-info/ 8 | build/ 9 | dist/ 10 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | try: 2 | from setuptools import setup 3 | except ImportError: 4 | from distutils.core import setup 5 | 6 | setup( 7 | name='CIO', 8 | version='1.0', 9 | description='Contact Invariant Optimization', 10 | url='https://github.com/carismoses/CIO', 11 | author='Caris Moses', 12 | author_email='carism@mit.edu', 13 | packages=['cio',], 14 | python_requires='>3.0', 15 | install_requires=[ 16 | 'imageio', 17 | 'scipy', 18 | 'jupyter', 19 | 'matplotlib' 20 | ], 21 | long_description=open('README.md').read(), 22 | ) 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CIO 2 | 3 | This package implements Contact Invariant Optimization by Mordatch, et al. 4 | 5 | ## Install 6 | 7 | This package has been tested with the following installation methods on Mac OS X 10.13.6 8 | 9 | ### Local Installation 10 | If you plan on frequently interacting with the source code, or you would like to follow along with a notebook tutorial, perform a local installation with these steps 11 | ``` 12 | git clone git@github.com:carismoses/CIO.git 13 | cd CIO 14 | pip install -v -e. 15 | ``` 16 | 17 | ### Git Installation 18 | If you just want to be able to import this package and don't plan on changing anything, you can perform the following installation 19 | ``` 20 | pip install git+git://github.com/carismoses/CIO.git 21 | ``` 22 | 23 | ## Test 24 | If you followed the local installation, you can test a minimal example with the following commands 25 | ``` 26 | cd example 27 | python3 minimal.py 28 | ``` 29 | WARNING: This will take a while to run! 30 | 31 | ## Tutorial 32 | If you followed the local installation, you can follow a tutorial on how to use the package by launching a jupyter notebook with the following command 33 | ``` 34 | jupyter notebook notebook_tutorial.ipynb 35 | ``` 36 | 37 | ## Tips 38 | If you followed the local installation, running the minimal example with the following command will launch pdb (The Python Debugger) 39 | ``` 40 | python3 minimal.py --debug 41 | ``` 42 | To create a breakpoint, the paths have to start from the root of the source code. For example to set a breakpoint at line 80 in CIO/cio/world.py use the following command within pdb 43 | ``` 44 | break cio/world:80 45 | ``` 46 | -------------------------------------------------------------------------------- /example/minimal.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cio.world import World, Circle 3 | from cio.params import Params, StageWeights 4 | from cio.optimization import CIO 5 | from cio.util import save_run, visualize_result, Position, Contact 6 | import argparse 7 | from collections import OrderedDict 8 | 9 | def main(args): 10 | if args.debug: 11 | import pdb; pdb.set_trace() 12 | 13 | # objects 14 | radius = 5.0 15 | manip_obj = Circle(radius, Position(5.0,radius)) 16 | finger0 = Circle(1.0, Position(-5.0, -5.0)) 17 | finger1 = Circle(1.0, Position(15.0, -5.0)) 18 | 19 | # initial contact information 20 | contact_state = OrderedDict([(finger0, Contact(f=(0.0, 0.0), ro=(-7., -7.), c=.5)), 21 | (finger1, Contact(f=(0.0, 0.0), ro=(7., -7.), c=.5))]) 22 | 23 | goals = [Position(5.0, 20.0)] 24 | 25 | world = World(manip_obj, [finger0, finger1], contact_state) 26 | 27 | # optimization parameters 28 | p = Params(world) 29 | 30 | # just get the cost of a single trajectory 31 | if args.single: 32 | stage_info = CIO(goals, world, p, single=True) 33 | # run the optimization 34 | else: 35 | stage_info = CIO(goals, world, p) 36 | 37 | # save the run to a .pickle file 38 | if args.save: 39 | save_run(args.save, p, world, stage_info) 40 | 41 | if __name__ == '__main__': 42 | parser = argparse.ArgumentParser() 43 | parser.add_argument('--debug', action='store_true') 44 | parser.add_argument('--single', action='store_true') 45 | parser.add_argument('--save', type=str) 46 | args = parser.parse_args() 47 | main(args) 48 | -------------------------------------------------------------------------------- /cio/params.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from collections import namedtuple 3 | 4 | StageWeights = namedtuple('StageWeights', 'w_CI w_physics w_kinematics w_task') 5 | """ 6 | Parameters 7 | ---------- 8 | w_CI : float 9 | the weight for the L_CI function 10 | w_physics : float 11 | the weight for the L_physics function 12 | w_kinematics : float 13 | the weight for the L_kinematics function 14 | w_task : float 15 | the weight for the L_task function 16 | """ 17 | 18 | class Params(object): 19 | def __init__(self, world, K=10, delT=0.05, delT_phase=0.5, mass=1.0, 20 | mu=0.9, lamb=10**-3, stage_weights=[StageWeights(w_CI=0.1, w_physics=0.1, w_kinematics=0.0, w_task=1.0), 21 | StageWeights(w_CI=10.**1, w_physics=10.**0, w_kinematics=0., w_task=10.**1)], init_traj=None): 22 | """ 23 | Parameters 24 | ---------- 25 | world : world.World 26 | an object representing the objects in the world 27 | K : int, optional 28 | the number of phases used to represent the trajectory 29 | delT : float, optional 30 | the time step used to calculate derivates using finite differences 31 | delT_phase : float, optional 32 | the length of time a phase lasts 33 | mass : float, optional 34 | the mass (kg) of the manipulated object 35 | mu : float, optional 36 | the coefficient of friction 37 | lamb : float, optional 38 | a regularizer that keeps accelerations and applied forces small 39 | stage_weights : list of StageWeights, optional 40 | the list of weights used during each optimization stage. the length of this list represents the number of optimization stages 41 | init_traj : function that takes in an initial state 42 | """ 43 | self.K = K 44 | self.delT = delT 45 | self.delT_phase = delT_phase 46 | self.mass = mass 47 | self.mu = mu 48 | self.lamb = lamb 49 | self.stage_weights = stage_weights 50 | 51 | ## DERIVED PARAMETERS 52 | self.N = len(world.contact_state) 53 | self.steps_per_phase = int(self.delT_phase/self.delT) 54 | self.T_steps = self.K*self.steps_per_phase 55 | self.T_final = self.K*self.delT_phase 56 | # each dynamic object has a 2D pose and vel and each contact surface has 5 associated vars 57 | self.len_s = int(6*len(world.get_all_objects()) + self.N*5) 58 | # add accelerations of dynamic objects 59 | self.len_s_aug = int(self.len_s + 3.*len(world.get_all_objects())) 60 | self.len_S = int(self.len_s*self.K) 61 | self.len_S_aug = int(self.len_s_aug*self.K*self.steps_per_phase) 62 | 63 | def print_stage_weights(self, stage): 64 | print('PHASE PARAMETERS:') 65 | print(' w_CI:', self.stage_weights[stage].w_CI) 66 | print(' w_kinematics:', self.stage_weights[stage].w_kinematics) 67 | print(' w_physics:', self.stage_weights[stage].w_physics) 68 | print(' w_task:', self.stage_weights[stage].w_task) 69 | print(' lambda:', self.lamb) 70 | -------------------------------------------------------------------------------- /cio/optimization.py: -------------------------------------------------------------------------------- 1 | # CIO implementation 2 | from scipy.optimize import fmin_l_bfgs_b, minimize 3 | import numpy as np 4 | from cio.world import Position, LinearVelocity 5 | from cio.util import print_final, visualize_result, get_bounds, add_noise, save_run, normalize, generate_world_traj 6 | 7 | #### MAIN OBJECTIVE FUNCTION #### 8 | def L(S, goals, world, p, stage=0): 9 | def L_CI(t, world_t): 10 | cost = 0 11 | for (ci, (cont_obj, cont)) in enumerate(world_t.contact_state.items()): 12 | cost += cont.c*(np.linalg.norm(world_t.e_O[ci])**2 + 13 | np.linalg.norm(world_t.e_H[ci])**2 + 14 | np.linalg.norm(world_t.e_dot_O[ci])**2 + 15 | np.linalg.norm(world_t.e_dot_H[ci])**2) 16 | return cost 17 | 18 | # includes 1) limits on finger and arm joint angles (doesn't apply) 19 | # 2) distance from fingertips to palms limit (doesn't apply) 20 | # 3) collisions between fingers 21 | def L_kinematics(t, world_t): 22 | cost = 0 23 | # any overlap between objects is penalized 24 | all_objects = world_t.get_all_objects() 25 | obj_num = 0 26 | while obj_num < len(world_t.get_all_objects()): 27 | for col_object in all_objects[obj_num+1:]: 28 | col_dist = all_objects[obj_num].check_collisions(col_object) 29 | cost += col_dist 30 | obj_num += 1 31 | return cost 32 | 33 | def L_physics(t, world_t): 34 | # calculate sum of forces on object 35 | f_tot = [0.0, 0.0] 36 | for cont in world_t.contact_state.values(): 37 | f_tot = np.add(f_tot, cont.c*cont.f) 38 | 39 | # calc change in linear momentum 40 | oa = world_t.manip_obj.accel 41 | p_dot = np.multiply(p.mass,[oa.x,oa.y]) 42 | newton_cost = np.linalg.norm(f_tot - p_dot)**2 43 | 44 | force_reg_cost = 0 45 | for cont in world_t.contact_state.values(): 46 | force_reg_cost += np.linalg.norm(cont.f)**2 47 | force_reg_cost = p.lamb*force_reg_cost 48 | 49 | # calc L_cone 50 | cone_cost = 0.0 51 | for (j,(cont_obj, cont)) in enumerate(world_t.contact_state.items()): 52 | n = cont_obj.get_surface_normal(world_t.pi_H[j]) 53 | f_n = normalize(cont.f) 54 | angle = np.arccos(np.dot(f_n, n)) 55 | cone_cost += max(angle - np.arctan(p.mu), 0)**2 56 | return force_reg_cost + newton_cost + cone_cost 57 | 58 | def L_task(t, world_t): 59 | # task constraint: get object to desired pos 60 | I = 1 if t == (p.T_steps-1) else 0 61 | task_cost = 0 62 | for goal in goals: 63 | if type(goal) == Position: 64 | obj_pos = [world_t.manip_obj.pose.x, world_t.manip_obj.pose.y] 65 | goal_pos = [goal.x, goal.y] 66 | task_cost += I*np.linalg.norm(np.subtract(obj_pos, goal_pos))**2 67 | elif type(goal) == LinearVelocity: 68 | obj_vel = [world_t.manip_obj.vel.x, world_t.manip_obj.vel.y] 69 | goal_vel = [goal.x, goal.y] 70 | task_cost += I*np.linalg.norm(np.subtract(obj_vel, goal_vel))**2 71 | 72 | # small acceleration constraint (supposed to keep hand accel small, but 73 | # don't have a central hand so use grippers individually) 74 | accel_cost = 0.0 75 | for obj in world_t.get_all_objects(): 76 | accel_cost += np.linalg.norm([obj.accel.x, obj.accel.y])**2 77 | accel_cost = p.lamb*accel_cost 78 | 79 | return accel_cost + task_cost 80 | 81 | world_traj = generate_world_traj(S, world, p) 82 | total_cost, ci, phys, kinem, task = 0.0, 0.0, 0.0, 0.0, 0.0 83 | for (t, world_t) in enumerate(world_traj): 84 | ci += p.stage_weights[stage].w_CI*L_CI(t, world_t) 85 | phys += p.stage_weights[stage].w_physics*L_physics(t, world_t) 86 | kinem += 0.#p.stage_weights[stage].w_kinematics*L_kinematics(t, world_t) 87 | task += p.stage_weights[stage].w_task*L_task(t, world_t) 88 | total_cost = ci + phys + kinem + task 89 | 90 | global function_costs 91 | function_costs = [ci, phys, kinem, task] 92 | return total_cost 93 | 94 | #### MAIN FUNCTION #### 95 | from collections import namedtuple 96 | StageInfo = namedtuple('StageInfo', 's0, x_final final_cost nit all_final_costs') 97 | def CIO(goals, world, p, single=False, start_stage=0, traj_data=None, gif_tag=''): 98 | if single: 99 | # FOR TESTING A SINGLE traj 100 | S = world.traj_func(world, goals, p, traj_data) 101 | S = add_noise(S) 102 | visualize_result(world, goals, p, 'initial'+gif_tag+'.gif', S) 103 | tot_cost = L(S, goals, world, p, start_stage) 104 | print_final(*function_costs) 105 | return {} 106 | 107 | S = world.traj_func(world, goals, p, traj_data) 108 | if start_stage == 0: 109 | S = add_noise(S) 110 | visualize_result(world, goals, p, 'initial'+gif_tag+'.gif', S) 111 | tot_cost = L(S, goals, world, p) 112 | print_final(*function_costs) 113 | 114 | bounds = get_bounds(world, p) 115 | ret_info = {} 116 | x_init = S 117 | for stage in range(start_stage, len(p.stage_weights)): 118 | print('BEGINNING PHASE:', stage) 119 | p.print_stage_weights(stage) 120 | res = minimize(fun=L, x0=x_init, args=(goals, world, p, stage), 121 | method='L-BFGS-B', bounds=bounds, options={'eps': 10**-3, 'ftol': 1}) 122 | x_final = res['x'] 123 | nit = res['nit'] 124 | final_cost = res['fun'] 125 | 126 | visualize_result(world, goals, p, 'stage_{}'.format(stage)+gif_tag+'.gif', x_final) 127 | print_final(*function_costs) 128 | all_final_costs = function_costs 129 | ret_info[stage] = StageInfo(world.s0, x_final, final_cost, nit, all_final_costs) 130 | x_init = x_final 131 | return ret_info 132 | -------------------------------------------------------------------------------- /cio/world.py: -------------------------------------------------------------------------------- 1 | import pdb 2 | import numpy as np 3 | from cio.util import calc_deriv, get_dist, normalize, stationary_traj, Position, \ 4 | Pose, LinearVelocity, Velocity, Acceleration, Contact 5 | 6 | class World(object): 7 | def __init__(self, manip_obj, fingers, contact_state, traj_func=stationary_traj): 8 | """ 9 | World Parameters 10 | ----- 11 | manip_obj : Circle 12 | the manipulated object 13 | fingers : list of Circle 14 | a list of Circle objects representing fingers 15 | contact_state : OrderedDict[Circle:Contact] 16 | a dictionary describing the contact state for each finger 17 | """ 18 | self.manip_obj = manip_obj 19 | self.fingers = fingers 20 | self.contact_state = contact_state 21 | self.traj_func = traj_func 22 | self.s0 = self.get_vars() 23 | 24 | self.pi_O = {} 25 | self.pi_H = {} 26 | self.e_O = {} 27 | self.e_H = {} 28 | self.e_dot_O = {} 29 | self.e_dot_H = {} 30 | 31 | def set_dynamics(self, obj_index, pose, vel, accel): 32 | objs = self.get_all_objects() 33 | objs[obj_index].set_dynamics(pose, vel, accel) 34 | 35 | def set_contact_state(self, obj_index, f, ro, c): 36 | cont_objs = list(self.contact_state) 37 | self.contact_state[cont_objs[obj_index]] = Contact(f=f, ro=ro, c=c) 38 | 39 | def set_e_vars(self, world_tm1, p): 40 | object_pose = np.array([self.manip_obj.pose.x, self.manip_obj.pose.y]) 41 | 42 | for (ci, (cont_obj, cont)) in enumerate(self.contact_state.items()): 43 | r = np.add(cont.ro, object_pose) 44 | self.pi_H[ci] = cont_obj.project_point(r) 45 | self.pi_O[ci] = self.manip_obj.project_point(r) 46 | self.e_H[ci] = np.subtract(self.pi_H[ci], r) 47 | self.e_O[ci] = np.subtract(self.pi_O[ci],r) 48 | 49 | if world_tm1 is None: 50 | self.e_dot_H[ci] = np.array([0., 0.]) 51 | self.e_dot_O[ci] = np.array([0., 0.]) 52 | else: 53 | self.e_dot_H[ci] = calc_deriv(self.e_H[ci],world_tm1.e_H[ci],p.delT) 54 | self.e_dot_O[ci] = calc_deriv(self.e_O[ci],world_tm1.e_O[ci],p.delT) 55 | 56 | def get_vars(self): 57 | s0 = np.array([]) 58 | 59 | # fill in object poses and velocities 60 | for object in self.get_all_objects(): 61 | s0 = np.concatenate([s0,object.pose]) 62 | s0 = np.concatenate([s0,object.vel]) 63 | 64 | # fill in contact info 65 | for cont_obj in self.contact_state: 66 | s0 = np.concatenate([s0,self.contact_state[cont_obj].f]) 67 | s0 = np.concatenate([s0,self.contact_state[cont_obj].ro]) 68 | s0 = np.concatenate([s0,[self.contact_state[cont_obj].c]]) 69 | 70 | return s0 71 | 72 | def get_all_objects(self): 73 | return [self.manip_obj] + self.fingers 74 | 75 | # world origin is left bottom with 0 deg being along the x-axis (+ going ccw), all poses are in world frame 76 | class Object(object): 77 | def __init__(self, pose = Pose(0.0,0.0,0.0), vel = Velocity(0.0, 0.0, 0.0), 78 | step_size = 0.5): 79 | self.pose = pose 80 | self.vel = vel 81 | self.step_size = step_size 82 | self.rad_bounds = 1e-1 83 | 84 | self.accel = None 85 | 86 | def set_dynamics(self, pose, vel, accel): 87 | self.pose = Pose(*pose) 88 | self.vel = Velocity(*vel) 89 | self.accel = Acceleration(*accel) 90 | 91 | def check_collisions(self, col_object): 92 | pts = self.discretize() 93 | max_col_dist = 0.0 94 | for pt in pts: 95 | col_dist = col_object.check_inside(pt) 96 | if col_dist > max_col_dist: 97 | max_col_dist = col_dist 98 | return max_col_dist 99 | 100 | # get a normal vector in the world frame directed from the center of this object 101 | # to the given point 102 | def get_surface_normal(self, point): 103 | origin_to_point = np.subtract(point, [self.pose.x, self.pose.y]) 104 | n = normalize(origin_to_point) 105 | return n 106 | 107 | class Line(Object): 108 | def __init__(self, length, pos, vel = Velocity(0.0, 0.0, 0.0), step_size = 0.5): 109 | self.length = length 110 | pose = Pose(pos.x, pos.y, 0.) # not using orientations yet 111 | super(Line,self).__init__(pose, vel, step_size) 112 | 113 | def discretize(self): 114 | N_points = np.floor(self.length/self.step_size) + 1 115 | points = np.array((N_points,2)) 116 | points[0,:], points[N_points-1,:] = self.get_endpoints() 117 | for i in range(1,N_points-1): 118 | points[i,:] = points[i-1,:] + self.step_size*np.array((np.cos(self.pose.theta), np.sin(self.angle))) 119 | return points 120 | 121 | def check_inside(self, point): 122 | pass #TODO 123 | 124 | def get_endpoints(self): 125 | p0 = np.array([self.pose.x, self.pose.y]) 126 | endpoint0 = p0 127 | endpoint1 = p0 + self.length*np.array((np.cos(self.pose.theta), np.sin(self.pose.theta))) 128 | return (endpoint0, endpoint1) 129 | 130 | def line_eqn(self): 131 | # check for near straight lines 132 | # line close to horizontal 133 | if abs(self.pose.theta) < self.rad_bounds or abs(self.pose.theta - np.pi) < self.rad_bounds: 134 | a = 0. 135 | b = 1. 136 | c = -self.pose.y 137 | # line close to vertical 138 | elif abs(self.pose.theta - np.pi/2.) < self.rad_bounds or abs(self.pose.theta - 3.*np.pi/2.) < self.rad_bounds: 139 | a = 1. 140 | b = 0. 141 | c = -self.pose.x 142 | else: 143 | slope = np.sin(self.pose.theta)/np.cos(self.pose.theta) 144 | int = self.pose.y - slope*self.pose.x 145 | a = -slope 146 | b = 1 147 | c = -int 148 | return a,b,c 149 | 150 | # project a given point onto this line (or endpoint) 151 | # TODO: smooth out endpoints? 152 | def project_point(self, point): 153 | a,b,c = self.line_eqn() 154 | n = b*point[0] - a*point[1] 155 | d = a**2 + b**2 156 | proj_point = np.array([b*n - a*c, -1*a*n - b*c])/d 157 | 158 | # check if point is on line segment, otherwise return closest endpoint 159 | endpoints = self.get_endpoints() 160 | if not (proj_point[0] <= max(endpoints[0][0], endpoints[1][0]) and 161 | proj_point[0] >= min(endpoints[0][0], endpoints[1][0]) and 162 | proj_point[1] <= max(endpoints[0][1], endpoints[1][1]) and 163 | proj_point[1] >= min(endpoints[0][1], endpoints[1][1])): 164 | 165 | dist0 = get_dist(proj_point, endpoints[0]) 166 | dist1 = get_dist(proj_point, endpoints[1]) 167 | if dist0 < dist1: 168 | proj_point = endpoints[0] 169 | else: 170 | proj_point = endpoints[1] 171 | return proj_point 172 | 173 | class Rectangle(Object): 174 | def __init__(self, width, height, pos, vel = LinearVelocity(0.0, 0.0), 175 | step_size = 0.5): 176 | self.width = width 177 | self.height = height 178 | pose = Pose(pos.x, pos.y, 0.0) # not using orientations yet 179 | vel = Velocity(vel.x, vel.y, 0.0) 180 | super(Rectangle,self).__init__(pose, vel, step_size) 181 | self.lines = self.make_lines() # rectangles are made up of 4 line objects 182 | 183 | def discretize(self): 184 | pass#TODO 185 | 186 | def check_inside(self, point): 187 | pass#TODO 188 | 189 | # defines list of lines in clockwise starting from left line 190 | def make_lines(self): 191 | lines = [] 192 | pose = self.pose 193 | angle = self.pose.theta 194 | length = self.height 195 | for i in range(4): 196 | line = Line(pose, angle, length) 197 | lines += [line] 198 | (_, pose) = line.get_endpoints() 199 | angle = angle - np.pi/2 200 | if length == self.height: 201 | length = self.width 202 | else: 203 | length = self.height 204 | return lines 205 | 206 | # return the closest projected point out of all rect surfaces 207 | # use a softmin instead of a hard min to make function smooth 208 | def project_point(self, point): 209 | k = 1.e4 210 | num_sides = len(self.lines) 211 | p_nearest = np.zeros((num_sides,2)) 212 | for j in range(num_sides): 213 | p_nearest[j,:] = self.lines[j].project_point(point) 214 | p_mat = np.tile(point.T, (num_sides, 1)) 215 | ones_vec = np.ones(num_sides) 216 | nu = np.divide(ones_vec, ones_vec + np.linalg.norm(p_mat-p_nearest,axis=1)**2*k) 217 | nu = nu/sum(nu) 218 | nu = np.tile(nu, (2,1)).T 219 | closest_point = sum(np.multiply(nu,p_nearest)) 220 | return closest_point 221 | 222 | 223 | class Circle(Object): 224 | def __init__(self, radius, pos, vel = LinearVelocity(x=0.0, y=0.0), 225 | step_size = 0.5): 226 | """ 227 | Circle Parameters 228 | ---------- 229 | radius : float 230 | the radius of the Circle 231 | pos : Position 232 | the position of the Circle 233 | vel : LinearVelocity, optional 234 | the linear velocity of the Circle, default = LinearVelocity(x=0) 235 | """ 236 | self.radius = radius 237 | pose = Pose(pos.x, pos.y, 0.0) 238 | vel = Velocity(vel.x, vel.y, 0.0) 239 | super(Circle,self).__init__(pose, vel, step_size) 240 | 241 | def discretize(self): 242 | pass#TODO 243 | 244 | def check_inside(self, point): 245 | pass#TODO 246 | 247 | # projects the given point onto the surface of this object 248 | def project_point(self, point): 249 | origin_to_point = np.subtract(point[:2], np.array([self.pose.x,self.pose.y])) 250 | origin_to_point /= np.linalg.norm(origin_to_point) 251 | closest_point = np.array([self.pose.x, self.pose.y]) + (origin_to_point * self.radius) 252 | return closest_point 253 | -------------------------------------------------------------------------------- /cio/util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pdb 3 | from scipy.interpolate import BPoly 4 | import imageio 5 | import matplotlib 6 | import matplotlib.pyplot as plt 7 | import shutil 8 | import tempfile 9 | import os 10 | import warnings 11 | import pickle 12 | from copy import deepcopy 13 | from collections import namedtuple 14 | warnings.filterwarnings("ignore") 15 | 16 | np.random.seed(0) 17 | 18 | """ 19 | Position Attributes 20 | ----- 21 | x : float 22 | x position 23 | y : float 24 | y position 25 | """ 26 | Position = namedtuple('Position', 'x y') 27 | Pose = namedtuple('Pose', 'x y theta') 28 | """ 29 | LinearVelocity Attributes 30 | ----- 31 | x : float 32 | linear velocity in x direction 33 | y : float 34 | linear velocity in y direction 35 | """ 36 | LinearVelocity = namedtuple('LinearVelocity', 'x y') 37 | Velocity = namedtuple('Velocity', 'x y theta') 38 | Acceleration = namedtuple('Acceleration', 'x y theta') 39 | """ 40 | Contact Attributes 41 | ----- 42 | f : tuple[2] 43 | x and y force 44 | ro : tuple[2] 45 | position of applied force in the frame of the manipulated object 46 | c : float in [0,1] 47 | the probability of being in contact 48 | """ 49 | Contact = namedtuple('Contact', 'f ro c') 50 | 51 | def generate_world_traj(S, world, p): 52 | # get dyanamic and contact info from S 53 | dyn_info = {} 54 | for (i, dyn_obj) in enumerate(world.get_all_objects()): 55 | poses, vels, accels = calc_obj_dynamics(world.s0, S, p, i) 56 | dyn_info[i] = [poses, vels, accels] 57 | 58 | dyn_offset = len(world.get_all_objects())*6 59 | cont_info = {} 60 | for (ci, cont_obj) in enumerate(world.contact_state): 61 | fs, ros, cs = get_contact_info(world.s0, S, p, ci, dyn_offset) 62 | cont_info[ci] = [fs, ros, cs] 63 | 64 | # fill into list of new worlds 65 | worlds = [] 66 | for t in range(p.T_steps+1): 67 | world_t = deepcopy(world) 68 | for i in range(len(world_t.get_all_objects())): 69 | world_t.set_dynamics(i,dyn_info[i][0][t], dyn_info[i][1][t], dyn_info[i][2][t]) 70 | for ci in range(len(world_t.contact_state)): 71 | world_t.set_contact_state(ci, cont_info[ci][0][:,t], cont_info[ci][1][:,t], cont_info[ci][2][t]) 72 | if t == 0: 73 | world_t.set_e_vars(None, p) 74 | else: 75 | world_t.set_e_vars(worlds[t-1], p) 76 | worlds += [world_t] 77 | return worlds 78 | 79 | def stationary_traj(world, goal, p, traj_data=None): 80 | S = np.zeros(p.len_S) 81 | for k in range(p.K): 82 | S[k*p.len_s:k*p.len_s+p.len_s] = world.get_vars() 83 | return S 84 | 85 | def save_run(file_name, goals, world, p, stage_info): 86 | fname = file_name + '.pickle' 87 | data = [goals, world, p, stage_info] 88 | with open(fname, 'wb') as handle: 89 | pickle.dump(data, handle) 90 | print('Saved run to', fname) 91 | 92 | def data_from_file(file_name): 93 | fname = file_name + '.pickle' 94 | with open(fname, 'rb') as handle: 95 | goals, world, p, stage_info = pickle.load(handle) 96 | print('Read data from', fname) 97 | return goals, world, p, stage_info 98 | 99 | def normalize(vec): 100 | mag = np.linalg.norm(vec) 101 | if mag == 0: 102 | return vec 103 | return np.divide(vec, mag) 104 | 105 | def calc_deriv(x1, x0, delta): 106 | return np.divide(np.subtract(x1,x0),delta) 107 | 108 | def get_dist(point0, point1): 109 | return np.linalg.norm(point1 - point0)**2 110 | 111 | def get_bounds(world, p): 112 | cis = [] 113 | dyn_obj_offset = 6*len(world.get_all_objects()) 114 | for i in range(len(world.contact_state)): 115 | cis += [dyn_obj_offset + 5*i + 4] 116 | bounds = [] 117 | for t in range(p.K): 118 | for v in range(p.len_s): 119 | if v in cis: 120 | bounds.append((0.,1.)) 121 | else: 122 | bounds.append((None,None)) 123 | return bounds 124 | 125 | def get_contact_info(s0, S, p, ci, dyn_offset): 126 | offset = dyn_offset+5*ci 127 | 128 | f_traj_K = [s0[offset:offset+2]] + [S[offset+k*p.len_s:offset+k*p.len_s+2] for k in range(p.K)] 129 | ro_traj_K = [s0[offset+2:offset+4]] + [S[offset+k*p.len_s+2:offset+k*p.len_s+4] for k in range(p.K)] 130 | c_traj_K = [s0[offset+4]] + [S[offset+k*p.len_s+4] for k in range(p.K)] 131 | 132 | # interpolate contacts, contact forces, and contact poses 133 | f_traj_T = np.zeros((2,p.T_steps+1)) 134 | ro_traj_T = np.zeros((2,p.T_steps+1)) 135 | c_traj_T = np.zeros(p.T_steps+1) 136 | 137 | for k in range(1,p.K+1): 138 | f_left, ro_left, c_left = f_traj_K[k-1], ro_traj_K[k-1], c_traj_K[k-1] 139 | f_right, ro_right, c_right = f_traj_K[k], ro_traj_K[k], c_traj_K[k] 140 | 141 | # interpolate the contact forces (linear) 142 | f_traj_T[:,(k-1)*p.steps_per_phase:k*p.steps_per_phase+1] = linspace_vectors(f_left, f_right, p.steps_per_phase+1) 143 | 144 | # interpolate the contact poses (linear) 145 | ro_traj_T[:,(k-1)*p.steps_per_phase:k*p.steps_per_phase+1] = linspace_vectors(ro_left, ro_right, p.steps_per_phase+1) 146 | 147 | for t in range((k-1)*p.steps_per_phase,k*p.steps_per_phase+1): 148 | c_traj_T[t] = c_traj_K[k] 149 | 150 | return f_traj_T, ro_traj_T, c_traj_T 151 | 152 | def calc_obj_dynamics(s0, S, p, index): 153 | # get pose and vel traj from S 154 | offset = 6*index 155 | pose_traj_K = [s0[offset:offset+3]] + [S[offset+k*p.len_s:offset+k*p.len_s+3] for k in range(p.K)] 156 | vel_traj_K = [s0[offset+3:offset+6]] + [S[offset+k*p.len_s+3:offset+k*p.len_s+6] for k in range(p.K)] 157 | 158 | # make spline functions 159 | spline_funcs = [] 160 | for dim in range(3): 161 | x = np.linspace(0.,p.T_final, p.K+1) 162 | y = np.zeros((p.K+1,2)) 163 | for k in range(p.K+1): 164 | y[k,:] = pose_traj_K[k][dim], vel_traj_K[k][dim] 165 | spline_funcs += [BPoly.from_derivatives(x,y,orders=3, extrapolate=False)] 166 | 167 | # use to interpolate pose 168 | pose_traj_T = [] 169 | k = 0 170 | times = np.linspace(0.,p.T_final, p.T_steps+1) 171 | for t in times: 172 | if not t % p.delT_phase: # this is a phase 173 | pose_traj_T += [pose_traj_K[k]] 174 | k += 1 175 | else: # get from spline 176 | pose_traj_T += [[spline_funcs[0](t), spline_funcs[1](t), spline_funcs[2](t)]] 177 | 178 | # use FD to get velocities and accels 179 | vel_traj_T = [np.zeros(3)] 180 | for t in range(1,p.T_steps+1): 181 | p_tm1 = pose_traj_T[t-1] 182 | p_t= pose_traj_T[t] 183 | vel_traj_T += [calc_deriv(p_t, p_tm1, p.delT)] 184 | 185 | accel_traj_T = [np.zeros(3)] 186 | for t in range(1,p.T_steps+1): 187 | v_tm1 = vel_traj_T[t-1] 188 | v_t = vel_traj_T[t] 189 | accel_traj_T += [calc_deriv(v_t, v_tm1, p.delT)] 190 | 191 | return pose_traj_T, vel_traj_T, accel_traj_T 192 | 193 | def linspace_vectors(vec0, vec1, num_steps): 194 | l = len(vec0) 195 | out_vec = np.zeros((l, num_steps)) 196 | for j in range(l): 197 | left = vec0[j] 198 | right = vec1[j] 199 | out_vec[j,:] = np.linspace(left, right, num_steps) 200 | return out_vec 201 | 202 | def add_noise(vec): 203 | # perturb all vars by gaussian noise 204 | mean = 0. 205 | var = 10.**-2 206 | for j in range(len(vec)): 207 | vec[j] += np.random.normal(mean, var) 208 | 209 | return vec 210 | 211 | ### FOR PRINTING AND VISUALIZING 212 | def print_final(ci, phys, kinem, task): 213 | print('TRAJECTORY COSTS:') 214 | print(' CI: ', ci) 215 | print(' kinematics: ', kinem) 216 | print(' physics: ', phys) 217 | print(' task: ', task) 218 | print(' TOTAL: ', ci + kinem + phys + task) 219 | 220 | def visualize_result(world, goals, p, outfile, S=None): 221 | if S is None: 222 | S = world.traj_func(world, goals, p) 223 | world_traj = generate_world_traj(S, world, p) 224 | 225 | temp_dirpath = tempfile.mkdtemp() 226 | image_filenames = [] 227 | 228 | worlds = [world] + world_traj 229 | for (t,world_t) in enumerate(worlds): 230 | plt.figure() 231 | 232 | object = world_t.manip_obj 233 | 234 | f_contact = np.array([0.0, 0.0]) 235 | for cont in world_t.contact_state.values(): 236 | f_contact += cont.c*np.array(cont.f) 237 | 238 | obj_pose = object.pose 239 | 240 | for (cont_obj, cont) in world_t.contact_state.items(): 241 | # get ro in world frame 242 | r = cont.ro + np.array([obj_pose.x, obj_pose.y]) 243 | rj_circ = plt.Circle(r, 1., fc='blue', alpha=cont.c) 244 | plt.gca().add_patch(rj_circ) 245 | 246 | hand_circ = plt.Circle([cont_obj.pose.x, cont_obj.pose.y], cont_obj.radius, fc='red') 247 | plt.gca().add_patch(hand_circ) 248 | 249 | plt.arrow(r[0], r[1], cont.f[0], cont.f[1], 250 | head_width=0.5, head_length=1., fc='k', ec='k') 251 | 252 | plt.plot([cont_obj.pose.x, r[0]], [cont_obj.pose.y, r[1]], c='black', linewidth=1.) 253 | try: 254 | rect = plt.Rectangle([obj_pose.x, obj_pose.y], object.width, object.height, fc='r') 255 | plt.gca().add_patch(rect) 256 | except AttributeError: 257 | circ = plt.Circle([obj_pose.x, obj_pose.y], object.radius, fc='r') 258 | plt.gca().add_patch(circ) 259 | 260 | # find position goal and print 261 | for goal in goals: 262 | if type(goal) == Position: 263 | goal_circ = plt.Circle(goal[:2], 1., fc='g') 264 | plt.gca().add_patch(goal_circ) 265 | 266 | ''' 267 | # plots the sum of the contact forces at object center 268 | plt.arrow(obj_pose.x, obj_pose.y, f_contact[0], f_contact[1], 269 | head_width=0.5, head_length=1., fc='k', ec='k') 270 | ''' 271 | plt.xlim((-20., 30)) 272 | plt.ylim((-10., 30)) # the second arg (max) is set relative to xlim and ylim.min 273 | plt.tight_layout() 274 | #plt.axes().set_aspect('equal', 'datalim') 275 | image_filename = os.path.join(temp_dirpath, '{}.png'.format(t)) 276 | plt.savefig(image_filename) 277 | plt.close() 278 | image_filenames.append(image_filename) 279 | images = [imageio.imread(filename) for filename in image_filenames] 280 | try: 281 | os.mkdir('output') 282 | except: 283 | pass 284 | outfile = 'output/'+outfile 285 | imageio.mimsave(outfile, images, fps=10) 286 | print("Wrote out to {}.".format(outfile)) 287 | 288 | shutil.rmtree(temp_dirpath) 289 | -------------------------------------------------------------------------------- /notebook_tutorial.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Contact Invariant Optimization\n", 8 | "\n", 9 | "This notebook gives an overview of [Contact Invariant Optimization]() (CIO) and an implementation of the method.\n", 10 | "\n", 11 | "Solving for manipulation trajectories involving contact is a notoriously difficult optimization problem due to discontinuities introduced by the hybrid space. CIO attempts to solve this issue by smoothing the transitions between contact dynamics and free space dynamics.\n", 12 | "\n", 13 | "CIO tries to solve the problem of finding a trajectory which manipulates an object into a desired state. The basis of CIO is the objective function which constrains the desired motion of the manipulated object and the manipulator. CIO attempts to solve for the necessary places to make contact, the forces to apply at these contact points, and the trajectory of the manipulated object and fingers. The paper describes all of the constraints used in the final optimization (L-BFGS). Here is a video of the kinds of manipulation plans CIO is able to find." 14 | ] 15 | }, 16 | { 17 | "cell_type": "code", 18 | "execution_count": null, 19 | "metadata": {}, 20 | "outputs": [], 21 | "source": [ 22 | "from IPython.lib.display import YouTubeVideo\n", 23 | "YouTubeVideo('Gzt2UoxYfAQ')" 24 | ] 25 | }, 26 | { 27 | "cell_type": "markdown", 28 | "metadata": {}, 29 | "source": [ 30 | "# State\n", 31 | "\n", 32 | "The manipulated object is a Circle with a position, $p_O$, and linear velocity $\\dot{p}_O$. These make up the state variable for the object, $s_O = [p_O, \\dot{p}_O]$." 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": null, 38 | "metadata": {}, 39 | "outputs": [], 40 | "source": [ 41 | "from cio.world import Circle\n", 42 | "from cio.util import Position\n", 43 | "\"\"\"\n", 44 | "Position Attributes\n", 45 | "-----\n", 46 | "x : float\n", 47 | " x position\n", 48 | "y : float\n", 49 | " y position\n", 50 | "\n", 51 | "LinearVelocity Attributes\n", 52 | "-----\n", 53 | "x : float\n", 54 | " linear velocity in x direction\n", 55 | "y : float\n", 56 | " linear velocity in y direction\n", 57 | "\n", 58 | "Circle Parameters\n", 59 | "------\n", 60 | "radius : float\n", 61 | " the radius of the Circle\n", 62 | "pos : Position\n", 63 | " the position of the Circle\n", 64 | "vel : LinearVelocity, optional\n", 65 | " the linear velocity of the Circle, default = LinearVelocity(x=0.0, y=0.0)\n", 66 | "\"\"\"\n", 67 | "radius = 5.0\n", 68 | "manip_obj = Circle(radius, Position(5.0,radius))" 69 | ] 70 | }, 71 | { 72 | "cell_type": "markdown", 73 | "metadata": {}, 74 | "source": [ 75 | "The manipulator is represented as a list of $N$ fingers. For finger $j$, the position is $p_j$ and the linear velocity is $\\dot{p}_j$. These make up the hand state variable, $s_H = [p_j \\dot{p}_j]$, $j \\in \\{1,...,N\\}$" 76 | ] 77 | }, 78 | { 79 | "cell_type": "code", 80 | "execution_count": null, 81 | "metadata": {}, 82 | "outputs": [], 83 | "source": [ 84 | "finger0 = Circle(1.0, Position(-5.0, -5.0))\n", 85 | "finger1 = Circle(1.0, Position(15.0, -5.0))" 86 | ] 87 | }, 88 | { 89 | "cell_type": "markdown", 90 | "metadata": {}, 91 | "source": [ 92 | "The contact state for each finger is defined as follows. $f_j$ is the force applied by finger $j$. $r_j^O$ is a position in the object frame which describes where $f_j$ is originating. $r_j$ is $r_j^O$ in the world frame. $c_j \\in [0,1]$ describes the nature of the contact. When $c_j = 1$, the finger should be in contact with the object, and when $c_j = 0$ it is not in contact. The figure below gives a visual for the contact parameters. These make up the contact state variable, $s_{CS} = [f_j, r_j^O c_j]$, $j \\in \\{1,...,N\\}$. In the current implementation we use only positions instead of full poses. The $x$ variables in the image below are full poses.\n", 93 | "![CIO State](notebook_files/contact_state.png)" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "from cio.world import Contact\n", 103 | "from collections import OrderedDict\n", 104 | "\"\"\"\n", 105 | "Contact Attributes\n", 106 | "-----\n", 107 | "f : tuple[2]\n", 108 | " x and y force\n", 109 | "ro : tuple[2]\n", 110 | " position of applied force in the frame of the manipulated object\n", 111 | "c : float in [0,1]\n", 112 | " the probability of being in contact\n", 113 | "\"\"\"\n", 114 | "contact_state = OrderedDict([(finger0, Contact(f=(0.0, 0.0), ro=(-7., -7.), c=.5)),\n", 115 | " (finger1, Contact(f=(0.0, 0.0), ro=(7., -7.), c=.5))])" 116 | ] 117 | }, 118 | { 119 | "cell_type": "markdown", 120 | "metadata": {}, 121 | "source": [ 122 | "The entire state is:\n", 123 | "\n", 124 | "$s = [s_O, s_H, s_{CS}]$\n", 125 | "\n", 126 | "The objective is to get the manipulated object to a specific Position." 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": null, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "goals = [Position(5.0, 20.0)]" 136 | ] 137 | }, 138 | { 139 | "cell_type": "markdown", 140 | "metadata": {}, 141 | "source": [ 142 | "The world is then initialized from this initial state." 143 | ] 144 | }, 145 | { 146 | "cell_type": "code", 147 | "execution_count": null, 148 | "metadata": {}, 149 | "outputs": [], 150 | "source": [ 151 | "from cio.world import World\n", 152 | "\"\"\"\n", 153 | "World Attributes (set with the following call)\n", 154 | "-----\n", 155 | "manip_obj : Circle\n", 156 | " the manipulated object\n", 157 | "fingers : list of Circle\n", 158 | " a list of Circle objects representing fingers\n", 159 | "contact_state : OrderedDict[Circle:Contact]\n", 160 | " a dictionary describing the contact state for each finger\n", 161 | "\"\"\"\n", 162 | "world = World(manip_obj, [finger0, finger1], contact_state)" 163 | ] 164 | }, 165 | { 166 | "cell_type": "markdown", 167 | "metadata": {}, 168 | "source": [ 169 | "# Trajectory\n", 170 | "\n", 171 | "A trajectory is represented using $K$ keyframe states. $s_0$ is constrained the be the initial state, making the decision variables that are optimized over:\n", 172 | "$S = [s_k], k \\in [1,K]$\n", 173 | "\n", 174 | "The decision variables are initialized to all correspond to the initial state of te system, with some added Gaussian noise.\n", 175 | "\n", 176 | "During the optimization we interpolate between these keyframe states to get a smooth trajectory, $s(t)$. Then this smoothed trajectory is used to calculate the cost of the objective function. Different interpolation methods are used for different parts of the state. The positions and velocities use cubic spline interpolation, the forces and $r$ values use linear interpolation, and the contact variables are piece-wise constant.\n", 177 | "\n", 178 | "Run the following two cell to generate a .gif which will visualize the initial interpolated trajectory (thanks Tom!!)." 179 | ] 180 | }, 181 | { 182 | "cell_type": "code", 183 | "execution_count": null, 184 | "metadata": { 185 | "scrolled": false 186 | }, 187 | "outputs": [], 188 | "source": [ 189 | "from cio.params import Params\n", 190 | "from cio.optimization import CIO\n", 191 | "import IPython\n", 192 | "\n", 193 | "p = Params(world) # will discuss the params later, for now use default values\n", 194 | "\n", 195 | "# the single argument just generates the trajectory, visualization, and calculates the cost\n", 196 | "CIO(goals, world, p, single=True) \n", 197 | "\n", 198 | "# run the .gif\n", 199 | "with open('output/initial.gif','rb') as f:\n", 200 | " display(IPython.display.Image(data=f.read(), format='png'))" 201 | ] 202 | }, 203 | { 204 | "cell_type": "markdown", 205 | "metadata": {}, 206 | "source": [ 207 | "The small red circles are the fingers and the large red circle is the manipulated object. The green circle is the goal position. The black line segments are from finger $j$ to the corresponding $r_j$, and the black arrow from $r_j$ is the applied force of contact $j$. The blue circles are the $r_j$ variables, and the shading represents the $c_j$ value (darker blue is $c_j$ closer to 1)." 208 | ] 209 | }, 210 | { 211 | "cell_type": "markdown", 212 | "metadata": {}, 213 | "source": [ 214 | "# Objective Cost Terms\n", 215 | "\n", 216 | "The objective function is a weighted sum over different objective functions which are each summed over all states, $s(t)$.\n", 217 | "\n", 218 | "## Contact-Invariant Cose: $L_{CI}(s_t)$\n", 219 | "\n", 220 | "A contact $j$ is active when $c_j$ is close to 1. When this is true, the corresponding gripper must be constrained to be in contact with the object. To calculate this we introduce new variables, $\\pi_O(r_j)$ and $\\pi_j(r_j)$. $\\pi_O(r_j)$ is $r_j$ projected onto finger $j$. $\\pi_j(r_j)$ is $r_j$ projected onto the manipulated object. This objective term aims to minimize the distance between these projections and $r_j$ when $c_j$ is close to 1. They also aim to minimize the rate at which these distances change to dicourage slipping.\n", 221 | "\n", 222 | "\n", 223 | "Below is a visualization of a CIO solution which only has $L_{CI}(s)$ as the objective function and $K=10$. In this visualization the $c_j$ values were also constrained to be 0.5. If they had been solved for in the optimization, they would have gone to 0 to minimize $L_{CI}(s)$.\n", 224 | "![ci_gif](notebook_files/L_ci_vis.gif)\n", 225 | "\n", 226 | "## Task Cost: $L_{task}(s_t)$\n", 227 | "\n", 228 | "This is the constraint which defines the task goal. First $T_b$ is defined as the time region in which you want the goal enforced. Then $h_b(s)$ is the component of the state which you are driving to drive to the goal, and $h^{star}_{b,t}$ is the desired value of the state components extracted from $h_b(s)$. It simply states that we want to minimize the distance between the desired and final state at some specified time step\n", 229 | "\n", 230 | "This term also regularizes the accelerations of the fingers and the object to encourage smoother trajectories. This regularization is parameterized by $\\lambda$.\n", 231 | "\n", 232 | "\n", 233 | "\n", 234 | "In the current implementation $h_b(s)$ can either be a desired Position or desired Velocity (this option hasn't been tested) of the object, and $T_b$ is just the final time step in the trajectory.\n", 235 | "\n", 236 | "Below is a visualization of a CIO solution which only has $L_{task}(s)$ as the objective function with $K=10$ and $\\lambda=10^{-3}$.\n", 237 | "![task_gif](notebook_files/L_task_vis.gif)\n", 238 | "\n", 239 | "## Physics Violation Cost: $L_{physics}(s_t)$\n", 240 | "\n", 241 | "The physics constraint accounts for the fact that forces and moments on the object from the contacts must balance with any change in linear or angular momentum of the object. This term also includes a regularization on the contact forces to keep them small, parameterized by the same $\\lambda$ as before. $f_{ext}$ and $m_{ext}$ in the equation below account for any external forces or moments such as those due to gravity.\n", 242 | "\n", 243 | "\n", 244 | "\n", 245 | "This term also has a friction cone component. For a coefficient of friction, $\\mu$, the contact force $j$ should lie within a friction cone centered at the line between finger $j$ and $r_j$. This friction cone is centered around the black line shown in the visualizations.\n", 246 | "\n", 247 | "\n", 248 | "\n", 249 | "In the current implementation, gravity and angular moment conservation are not incorporated.\n", 250 | "\n", 251 | "Below is a visualization of a CIO solution which only has $L_{task}(s)$ and $L_{physics}(s)$ as the objective function and $\\lambda=10^{-3}$, $\\mu=0.9$. The decision variables were initialized to the values shown in the previous .gif (with the $L_{task}$ cost). If there were only a physics cost then the object would not move.\n", 252 | "\n", 253 | "![phys_gif](notebook_files/L_task_phys_vis.gif)\n", 254 | "\n", 255 | "## Kinematic Violation: $L_{kinematics}(s_t)$\n", 256 | "\n", 257 | "This objective tries to minimize any violations of kinematic constraints, such as joint angles, the distance the fingers can extend from the hand, and collisions between any objects. None of these are implemented in this version. While this permits objects to collide, there is still an objective of making contact between the surfaces of fingers and objects due to the $L_{CI}(s)$ cost term.\n", 258 | "\n", 259 | "## Total Objective Function\n", 260 | "\n", 261 | "All of these terms are used in the final objective function which sums over time and each individual cost term.\n", 262 | "\n", 263 | "" 264 | ] 265 | }, 266 | { 267 | "cell_type": "markdown", 268 | "metadata": {}, 269 | "source": [ 270 | "# Optimization\n", 271 | "\n", 272 | "CIO makes use of the L-BFGS optimization method and occurs over 2 calls to L-BFGS. The decision variables for the second call are initialized to the resulting values from the first optimization. In each stage the optimization functions are weighted differently. The first stage uses the following weights.\n", 273 | "\n", 274 | "\n", 275 | "\n", 276 | "The second stage uses the following weights.\n", 277 | "\n", 278 | "\n", 279 | "\n", 280 | "Now you can test it yourself with the following call, and play the resulting .gifs.\n", 281 | "\n", 282 | "WARNING: This will take a while to run!" 283 | ] 284 | }, 285 | { 286 | "cell_type": "code", 287 | "execution_count": null, 288 | "metadata": {}, 289 | "outputs": [], 290 | "source": [ 291 | "CIO(goals, world, p)\n", 292 | "with open('output/stage_0.gif','rb') as f:\n", 293 | " display(IPython.display.Image(data=f.read(), format='png'))\n", 294 | "with open('output/stage_1.gif','rb') as f:\n", 295 | " display(IPython.display.Image(data=f.read(), format='png'))" 296 | ] 297 | }, 298 | { 299 | "cell_type": "markdown", 300 | "metadata": {}, 301 | "source": [ 302 | "# Parameters\n", 303 | "\n", 304 | "There are many parameters which can give very different optimization results. The Params() object which was initialized previously can take in arguments to override the default values. Below are the parameters used to initialize a Params() object initialized with their default values. Feel free to directly change them here." 305 | ] 306 | }, 307 | { 308 | "cell_type": "code", 309 | "execution_count": null, 310 | "metadata": {}, 311 | "outputs": [], 312 | "source": [ 313 | "\"\"\"\n", 314 | "StageWeights Attributes\n", 315 | "----------\n", 316 | "w_CI : float\n", 317 | " the weight for the L_CI function\n", 318 | "w_physics : float\n", 319 | " the weight for the L_physics function\n", 320 | "w_kinematics : float\n", 321 | " the weight for the L_kinematics function (this parameter doesn't affect anything)\n", 322 | "w_task : float\n", 323 | " the weight for the L_task function\n", 324 | "\n", 325 | "Params Parameters\n", 326 | "----------\n", 327 | "world : world.World\n", 328 | " an object representing the objects in the world\n", 329 | "K : int, optional\n", 330 | " the number of phases used to represent the trajectory\n", 331 | "delT : float, optional\n", 332 | " the time step used to calculate derivates using finite differences\n", 333 | "delT_phase : float, optional\n", 334 | " the length of time a phase lasts\n", 335 | "mass : float, optional\n", 336 | " the mass (kg) of the manipulated object\n", 337 | "mu : float, optional\n", 338 | " the coefficient of friction\n", 339 | "lamb : float, optional\n", 340 | " a regularizer that keeps accelerations and applied forces small\n", 341 | "stage_weights : list of StageWeights, optional\n", 342 | " the list of weights used during each optimization stage. the length of this list represents the number of optimization stages\n", 343 | "\n", 344 | "Below are the default values for all of the parameters\n", 345 | "\"\"\"\n", 346 | "from cio.params import StageWeights\n", 347 | "stage_weights=[StageWeights(w_CI=0.1, w_physics=0.1, w_kinematics=0.0, w_task=1.0),\n", 348 | " StageWeights(w_CI=10.**1, w_physics=10.**0, w_kinematics=0., w_task=10.**1)]\n", 349 | "p = Params(world, K=10, delT=0.05, delT_phase=0.5, mass=1.0,\n", 350 | " mu=0.9, lamb=10**-3, stage_weights=stage_weights)\n", 351 | "CIO(goals, world, p);" 352 | ] 353 | }, 354 | { 355 | "cell_type": "markdown", 356 | "metadata": {}, 357 | "source": [ 358 | "# Your Testing\n", 359 | "\n", 360 | "Now feel free to play around with CIO. Here are some ideas for the adventurous:\n", 361 | "- Change the initial world state \n", 362 | " - Where the objects are\n", 363 | " - The contact state values\n", 364 | " - The number of fingers\n", 365 | "- Change the parameters\n", 366 | " - Different stage weights\n", 367 | " - Different number of stages\n", 368 | " - Different coefficient of friction\n", 369 | "- Change the goal description\n", 370 | " - The goals parameter passed into CIO() is a list of constraints on the world state. Currently it can only take in a constraint on the Position and LinearVelocity of the manipulated object at the final time step, but it isn't too difficult to add more goal types.\n", 371 | " - Try sequential goals\n", 372 | " - Try constraining the goal velocity\n", 373 | "- Change other parts of the cost function\n", 374 | " - Try to add gravity\n", 375 | " \n", 376 | "The cost functions are all in CIO.py and named the same as above. They take in a time, t, and the world state at time t. See world.World() for what can be accessed in a world state.\n", 377 | " " 378 | ] 379 | } 380 | ], 381 | "metadata": { 382 | "kernelspec": { 383 | "display_name": "Python 3 (ipykernel)", 384 | "language": "python", 385 | "name": "python3" 386 | }, 387 | "language_info": { 388 | "codemirror_mode": { 389 | "name": "ipython", 390 | "version": 3 391 | }, 392 | "file_extension": ".py", 393 | "mimetype": "text/x-python", 394 | "name": "python", 395 | "nbconvert_exporter": "python", 396 | "pygments_lexer": "ipython3", 397 | "version": "3.9.12" 398 | } 399 | }, 400 | "nbformat": 4, 401 | "nbformat_minor": 2 402 | } 403 | --------------------------------------------------------------------------------