Commit f8052d13 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

reintroduce cal_test generating file...

reintroduce cal_test generating file (twins-controller#18)
parent 36366f1f
"""
Generate Plan fo rtesting calibration
"""
import gcode2contour as gc
import utils as tu
import pybullet as p
import time
from math import pi
import numpy as np
import warnings
import pickle
import copy
home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def plan(points, env, gui=False):
"""
Create plan which simply goines to certain points
"""
plan = tu.Plan(env)
n_arms = 1
arm = 0
# Got to each point
for p in points:
# Go to 5cm above the point
p_above = copy.deepcopy(p)
p_above[2] = p[2] + 0.05
jabove = tu.get_closest(env.ik_pos(p_above,0),home, max_diff=100)
go2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([go2above], arm)
# Go to the point
jpoint = tu.get_closest(env.ik_pos(p,0),home, max_diff=100)
go2point = plan.plan_to(jpoint, arm)
plan.appendTrajectory([go2point], arm)
# go to 5cm above point again
back2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([back2above], arm)
# try going to home pos
gohome = plan.plan_to(env.home, arm)
if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
print("done")
else:
raise Exception("cannot plan back to home position")
return plan
if __name__ == "__main__":
# 1arm real calibration
env_desc1 = tu.read_env('calibrations/r1_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc1, gui=True, home=home)
# Greedily plan first n contours
points = [
[ 0 , 0 , 0],
[-0.215, 0.215, 0],
[ 0 , 0.215, 0],
[ 0.215, 0.215, 0],
[ 0.215, 0 , 0],
[ 0.215, -0.215, 0],
[ 0 , -0.215, 0],
[-0.215, -0.215, 0],
[-0.215, 0 , 0],
]
plan1 = plan(points, env, gui=True)
plan1.save("cal_test.plan")
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment