def main():
world, robot, link_index, geom_index, obs_index = create_world()
q0 = [0, -2, 0]
qf = [3, 1, 3.14]
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
q0=q0,
qf=qf,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
# create guess and solve
n_grid = 10
guess = np.linspace(q0, qf, n_grid)
rst = trajopt.optimize(guess)
print(rst)
traj = rst['sol']
# show the results
vis.add('world', world)
robot.setConfig(traj[-1])
for i, qi in enumerate(traj[:-1]):
name = 'ghost%d' % i
vis.add(name, qi)
vis.setAttribute(name, 'type', 'Config')
vis.setColor(name, 0, 1, 0, 0.4)
vis.spin(100)
def __call__(self):
vis.lock()
#TODO: you may modify the world here. This line tests a sin wave.
pt[2] = 1 + math.sin(self.iteration*0.03)
vis.unlock()
#changes to the visualization with vis.X functions can done outside the lock
if (self.iteration % 100) == 0:
if (self.iteration / 100)%2 == 0:
vis.hide("some blinking transform")
vis.addText("text4","The transform was hidden")
vis.logPlotEvent('plot','hide')
else:
vis.hide("some blinking transform",False)
vis.addText("text4","The transform was shown")
vis.logPlotEvent('plot','show')
#this is another way of changing the point's data without needing a lock/unlock
#vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True)
#or
#vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)])
if self.iteration == 200:
vis.addText("text2","Going to hide the text for a second...")
if self.iteration == 400:
#use this to remove text
vis.clearText()
if self.iteration == 500:
vis.addText("text2","Text added back again")
vis.setColor("text2",1,0,0)
self.iteration += 1
def sampling_9(serial_num):
world_name = "flatworld"
world_file = "../../data/robot_model_files/worlds/" + world_name + ".xml"
robot_file = "../../data/robot_model_files/robots/irb120_icp.rob"
world = get_world(world_file, robot_file, visualize_robot=True)
robot = world.robot(0)
est_config_list = cal_est_config_list(robot, serial_num)
path = collision_checking(world, est_config_list, 0.001)
if path is False:
sys.exit("The calibration path exist self-collision.")
else:
print("The calibration path is collision free.")
vis.clear()
vis.add("world", world)
for i in range(len(est_config_list)):
vis.add("estimated configure" + str(i), est_config_list[i])
vis.setColor("estimated configure" + str(i), 1.0, 0.0, 0.1 * i, 0.5)
vis.spin(float('inf'))
df = pd.DataFrame(np.asarray(est_config_list)[:, 7:13] * (180. / np.pi))
df.to_csv("../../data/robot_configuration_files/configuration_record_" + serial_num + ".csv",
header=False, index=False)
time.sleep(1.)
est_config_list = est_config_list[0:]
controller = SampleController(est_config_list, robot, '192.168.1.178')
controller.start()
def __init__(self,endeffectors,constraints,traj):
vis.GLPluginInterface.__init__(self)
self.endeffectors = endeffectors
self.constraints = constraints
self.robot = self.constraints[0].robot
self.traj = traj
self.refConfig = self.robot.getConfig()
vis.add("ghost1",self.refConfig)
vis.setColor("ghost1",0,1,0,0.5)
#vis.hide("ghost1",False)
def bumpTrajectory():
relative_xforms = []
robot.setConfig(self.refConfig)
for e in self.endeffectors:
xform = vis.getItemConfig("ee_"+robot.link(e).getName())
T = (xform[:9],xform[9:])
T0 = robot.link(e).getTransform()
Trel = se3.mul(se3.inv(T0),T)
print "Relative transform of",e,"is",Trel
relative_xforms.append(Trel)
bumpTraj = cartesian_trajectory.cartesian_bump(self.robot,self.traj,self.constraints,relative_xforms,ee_relative=True,closest=True)
assert bumpTraj != None
vis.animate(("world",world.robot(0).getName()),bumpTraj)
self.refresh()
self.add_action(bumpTrajectory,"Bump trajectory",'b')
def show_friction_cone(folder="FrictionCones/StanfordMicroSpineUnit"):
V = Volume()
V.load(folder)
for C in V.convex_decomposition:
print "Convex decomposition"
print np.amin(C.points,axis=0)
print np.amax(C.points,axis=0)
Chull = np.vstack([V.convex_hull.points[u] for u in V.convex_hull.vertices])
#print Chull
Chull *= 0.05
for i,C in enumerate(V.convex_decomposition):
if len(V.convex_decomposition) == 1:
g = 0
else:
g = float(i)/(len(V.convex_decomposition)-1)
#for j,v in enumerate(C.vertices):
# vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05))
# vis.setColor("pt%d,%d"%(i,j),1,g,0)
# vis.hideLabel("pt%d,%d"%(i,j))
for j,pt in enumerate(C.points):
v = j
vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05))
vis.setColor("pt%d,%d"%(i,j),1,g,0)
vis.hideLabel("pt%d,%d"%(i,j))
for i in xrange(Chull.shape[0]):
vis.add("CH%d"%(i,),Chull[i,:].tolist())
vis.hideLabel("CH%d"%(i,))
vis.add("origin",(0,0,0))
vis.setColor("origin",0,1,0)
vis.dialog()
def RobotCOMPlot(SimRobot, vis):
COMPos_start = SimRobot.getCom()
COMPos_end = COMPos_start[:]
COMPos_end[2] = COMPos_end[2] - 7.50
vis.add("COM", Trajectory([0, 1], [COMPos_start, COMPos_end]))
vis.hideLabel("COM", True)
vis.setColor("COM", 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute("COM", 'width', 5.0)
def COM2IntersectionPlot(i, vis, COM_Pos, Intersection):
# This function is used to plot PIP from COM to intersections
Edge_Index = str(i)
vis.add("PIPEdgefromCOM:" + Edge_Index,
Trajectory([0, 1], [COM_Pos, Intersection]))
vis.hideLabel("PIPEdgefromCOM:" + Edge_Index, True)
vis.setAttribute("PIPEdgefromCOM:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgefromCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
def add_hm_to_klampt_vis(self, pcd_fname):
pc_xyzs, non_floor_points_xyz = self.parse_hm(pcd_fname)
pc_xyzs_as_list = pc_xyzs.flatten().astype("float")
pcloud = PointCloud()
pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list)
vis.add("pcloud", pcloud)
r, g, b, a = PCloudParser.PCLOUD_COLOR
vis.setColor("pcloud", r, g, b, a)
def convert(geom,type,label):
global a,b,atypes,btype
if label=='A':
vis.add(label,atypes[type])
vis.setColor(label,1,0,0,0.5)
a = atypes[type]
else:
vis.add(label,btypes[type])
vis.setColor(label,0,1,0,0.5)
b = btypes[type]
def runPlanner(runfunc, name):
global lastPlan
res = runfunc()
if res:
if lastPlan:
vis.remove(lastPlan)
vis.add(name, res)
vis.setColor(name, 1, 0, 0)
vis.animate(("world", robot.getName()), res)
lastPlan = name
def remesh(geom, label):
global a, b, Ta, Tb
if label == 'A':
a = a.convert(a.type(), 0.06)
vis.add(label, a)
vis.setColor(label, 1, 0, 0, 0.5)
a.setCurrentTransform(*Ta)
elif label == 'B':
b = b.convert(b.type(), 0.06)
vis.add(label, b)
vis.setColor(label, 0, 1, 0, 0.5)
b.setCurrentTransform(*Tb)
def convert(geom, type, label):
global a, b, atypes, btypes, Ta, Tb
if label == 'A':
vis.add(label, atypes[type])
vis.setColor(label, 1, 0, 0, 0.5)
a = atypes[type]
a.setCurrentTransform(*Ta)
else:
vis.add(label, btypes[type])
vis.setColor(label, 0, 1, 0, 0.5)
b = btypes[type]
b.setCurrentTransform(*Tb)
def test_height_around_endeff_costfn(self):
br_xyz = (8.759999999999984, 0.48, 0.0)
ret = self.step_seq_planner.height_around_endeff_costfn(br_xyz[0],
br_xyz[1],
debug=True)
sphere = klampt.GeometricPrimitive()
sphere.setSphere([br_xyz[0], br_xyz[1], 0.1],
parameters.STEPSEQ_HEIGHT_AROUND_ENDEFF_R)
vis.add("sphere", sphere)
vis.setColor("sphere", 0, 0, 1, 0.5)
print(ret)
def main():
world, robot, link_index, geom_index, obs_index = create_world()
# get start and target for the joints
joint_start = [
0.115, 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166, 1.3425,
-0.6365, 0.0981, -1.226, -2.0264, -3.0125, -1.3958, -1.9289, 2.9295,
0.5748, -3.137
]
joint_target = [
0.115, 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005, 1.5994,
-0.6929, -0.3338, -1.292, -1.9048, -2.6915, -1.2908, -1.7152, 1.3155,
0.6877, -0.0041
]
assert (len(link_index) == len(joint_start) == len(joint_target))
# generate guess
n_grid = 10 # resolution
guess = np.linspace(joint_start, joint_target, n_grid)
# create trajopt instance
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
joint_start,
joint_target,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
# create some stuff for visualization
config = np.array(robot.getConfig())
def update_config(q):
config[link_index] = q
def get_config(q):
cq = copy.copy(config)
cq[link_index] = q
return cq
rst = trajopt.optimize(guess)
sol = rst['sol']
vis.add('world', world)
dists = []
for i in range(1, n_grid -
1): # set to -2 so the robot stops there with collision
vis.add('ghost%d' % i, list(get_config(sol[i])))
vis.setColor('ghost%d' % i, 0, 1, 0, 0.3)
robot.setConfig(get_config(sol[i]))
update_config(sol[-1])
robot.setConfig(config)
vis.spin(float('inf'))
def check_collision(self):
for iT in range(self.world.numTerrains()):
collRT0 = self.collision_checker.robotTerrainCollisions(
self.world.robot(0), iT)
collision_flag = False
for i, j in collRT0:
collision_flag = True
strng = "Robot collides with " + j.getName()
vis.addText("textCol", strng)
vis.setColor("textCol", 1, 0, 0)
break
for iR in range(self.world.numRigidObjects()):
collRT2 = self.collision_checker.robotObjectCollisions(
self.world.robot(0), iR)
for i, j in collRT2:
if j.getName() != "goal":
collision_flag = True
strng = self.world.robot(
0).getName() + " collides with " + j.getName()
vis.addText("textCol", strng)
vis.setColor("textCol", 1, 0, 0)
if collision_flag:
vis.addText("textCol", "Collision")
vis.setColor("textCol", 1, 0.0, 0.0)
if not collision_flag:
vis.addText("textCol", "No collision")
vis.setColor("textCol", 0.4660, 0.6740, 0.1880)
return collision_flag
def __init__(self,endeffectors,constraints):
vis.GLPluginInterface.__init__(self)
self.endeffectors = endeffectors
self.constraints = constraints
self.robot = self.constraints[0].robot
self.goalConfig = self.robot.getConfig()
vis.add("ghost1",self.robot.getConfig())
vis.setColor("ghost1",0,1,0,0.5)
vis.add("ghost2",self.robot.getConfig())
vis.setColor("ghost2",1,0,0,0.5)
#vis.hide("ghost1",False)
#vis.hide("ghost2",False)
def goToWidget():
#first, set all constraints so they are fit at the robot's last solved configuration, and get the
#current workspace coordinates
vis.setItemConfig("ghost1",self.goalConfig)
self.robot.setConfig(self.goalConfig)
for e,d in zip(self.endeffectors,self.constraints):
d.matchDestination(*self.robot.link(e).getTransform())
wcur = config.getConfig(self.constraints)
wdest = []
for e in self.endeffectors:
xform = vis.getItemConfig("ee_"+robot.link(e).getName())
wdest += xform
print "Current workspace coords",wcur
print "Dest workspace coords",wdest
#Now interpolate
self.robot.setConfig(self.goalConfig)
traj1 = cartesian_trajectory.cartesian_interpolate_linear(robot,wcur,wdest,self.constraints,delta=1e-2,maximize=False)
self.robot.setConfig(self.goalConfig)
traj2 = cartesian_trajectory.cartesian_interpolate_bisect(robot,wcur,wdest,self.constraints,delta=1e-2)
self.robot.setConfig(self.goalConfig)
#traj3 = cartesian_trajectory.cartesian_path_interpolate(robot,[wcur,wdest],self.constraints,delta=1e-2,method='any',maximize=False)
traj3 = None
print "Method1 Method2 Method3:"
print " ",(traj1 != None), (traj2 != None), (traj3 != None)
if traj1: traj = traj1
elif traj2: traj = traj2
elif traj3: traj = traj3
else: traj = cartesian_trajectory.cartesian_interpolate_linear(robot,wcur,wdest,self.constraints,delta=1e-2,maximize=True)
if traj:
print "Result has",len(traj.milestones),"milestones"
self.goalConfig = traj.milestones[-1]
vis.setItemConfig(("world",world.robot(0).getName()),self.goalConfig)
vis.animate(("world",world.robot(0).getName()),traj,speed=0.2,endBehavior='loop')
vis.setItemConfig("ghost2",traj.milestones[-1])
vis.add("ee_trajectory",traj)
self.refresh()
self.add_action(goToWidget,"Go to widget",' ')
def main():
world, robot, link_index, geom_index, obs_index = create_world()
joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074]
joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988]
# compute the goal pose
config0 = robot.getConfig()
for lid, theta in zip(link_index, joint_target):
config0[lid] = theta
robot.setConfig(config0)
tR, tt = robot.link(link_index[-1]).getTransform()
print(f'target is {tR}, {tt}')
# create the trajopt instance
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
q0=joint_start,
qf=None,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
FINAL_POSE = True
if FINAL_POSE:
trajopt.add_pose_constraint(-1, link_index[-1], (tR, tt))
# add keep constraint
trajopt.add_orientation_constraint(None, link_index[-1], tR)
# generate guess
n_grid = 10 # resolution
guess = np.linspace(joint_start, joint_target, n_grid)
# create some stuff for visualization
config = np.array(robot.getConfig())
def update_config(q):
config[link_index] = q
def get_config(q):
cq = copy.copy(config)
cq[link_index] = q
return cq
rst = trajopt.optimize(guess)
sol = rst['sol']
vis.add('world', world)
for i in range(1, n_grid -
2): # set to -2 so the robot stops there with collision
vis.add('ghost%d' % i, list(get_config(sol[i])))
vis.setColor('ghost%d' % i, 0, 1, 0, 0.5)
update_config(sol[-1])
vis.spin(float('inf'))
def sample_config(self):
"""
:return: None
"""
self.Clink_set = np.array(
pd.read_csv(self.config_filename, header=None)).tolist()
for i in range(len(self.Clink_set)):
self.Clink_set[i] = np.multiply(self.Clink_set[i],
np.pi / 180.).tolist()
self.Clink_set[i] = [0] * 7 + self.Clink_set[i] + [0]
for i in range(len(self.Clink_set)):
vis.add("configure " + str(i), self.Clink_set[i])
vis.setColor("configure " + str(i), i * 0.15, 1, i * 0.05, 0.5)
self.robot.setConfig(self.Clink_set[i])
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
RowEnd = RowNo
for i in range(RowStart, RowEnd):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
TransName = "Step" + str(StepNo) + "Limb" + str(
LimbNo) + "TransPoint:" + str(i)
vis.add(TransName, point_start)
vis.hideLabel(TransName, True)
vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0,
1.0)
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
RowEnd = RowNo
Traj = []
for i in range(RowStart, RowEnd):
point_i = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_i[0] = ReachableContact_i[0]
point_i[1] = ReachableContact_i[1]
point_i[2] = ReachableContact_i[2]
Traj.append(point_i)
TransName = "Stage" + str(StepNo) + "LinkNo" + str(LimbNo) + "Path"
vis.add(TransName, Trajectory([0, 1], Traj))
vis.hideLabel(TransName, True)
vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0, 1.0)
def ContactDataPlot(vis, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
# RowEnd = min(RowNo, 99)
RowEnd = RowNo
for i in range(RowStart, RowEnd):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
vis.add("Point_" + str(i), point_start)
vis.hideLabel("Point_" + str(i), True)
vis.setColor("Point_" + str(i), 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
def ContactDataPlot(vis, StepNo, LimbNo, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
RowEnd = RowNo
for i in range(RowStart, RowEnd):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
ContactName = "Stage" + str(StepNo) + "LinkNo" + str(
LimbNo) + "Point:" + str(i)
vis.add(ContactName, point_start)
vis.hideLabel(ContactName, True)
vis.setColor(ContactName, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0,
1.0)
def main():
world, robot = load_world()
space = makeSpace(world, robot)
collision_check = lambda: (space.selfCollision(robot.getConfig(
))) == False and (space.envCollision(robot.getConfig()) == False)
# q0, qf = compute_initial_final(world, robot)
q0 = np.array([
0.0, 0.04143683792522413, -1.183867130629673, 2.0680756463305556,
3.745524327531242, 3.6939852943821956e-06, 0.08270468308944955, 0.0
])
qf = np.array([
0.0, 3.590769443639893, -0.8366423979290207, 1.5259988654642873,
5.59380779741848, 0.8157228966102285, 4.712401453217654, 0.0
])
robot.setConfig(q0)
q0 = q0[1:-1]
qf = qf[1:-1]
link_index = [1, 2, 3, 4, 5, 6]
geom_index = link_index
obs_index = [0, 1]
n_grid = 10 # resolution
guess = np.linspace(q0, qf, n_grid)
# guess += np.random.uniform(-0.1, 0.1, size=guess.shape)
vis.add("world", world)
# this one has no additional constraint so it should be easy to solve...
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'})
trajopt = KineTrajOpt(world,
robot,
q0,
qf,
config=config,
link_index=link_index,
obs_index=obs_index,
geom_index=geom_index)
rst = trajopt.optimize(guess)
sol = rst['sol']
for i in range(1, n_grid):
robot.setConfig([0] + sol[i].tolist() + [0])
print(collision_check())
vis.add('ghost%d' % i, [0] + list(sol[i]) + [0])
vis.setColor('ghost%d' % i, 0, 1, 0, 0.5)
vis.spin(float('inf'))
def visRoadMap(self):
vis.show()
edgeList = self.G.edges()
cnt = 0
for e in edgeList:
n1 = e[0]
n2 = e[1]
tr = trajectory.Trajectory()
tr.milestones.append([n1.x, n1.y, n1.z])
tr.milestones.append([n2.x, n2.y, n2.z])
fName = "a" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setAttribute(fName, "width", 0.5)
vis.setColor(fName, 0.4940, 0.1840, 0.5560)
cnt = cnt + 1
self.rmCnt = cnt
def WeightedContactDataPlot(vis, OptimalContact_data,
OptimalContactWeights_data):
scale = 1.0
for i in range(OptimalContact_data.size / 3):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = OptimalContact_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
point_end = [0.0, 0.0, 0.0]
ReachableContactWeight_i = OptimalContactWeights_data[i]
point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0]
point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1]
point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2]
print i
vis.add("PointWeights_" + str(i),
Trajectory([0, 1], [point_start, point_end]))
vis.hideLabel("PointWeights_" + str(i), True)
vis.setColor("PointWeights_" + str(i), 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute("PointWeights_" + str(i), 'width', 5.0)
def main():
world, robot, link_index, geom_index, obs_index = create_world()
joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074]
joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988]
# generate guess
n_grid = 10 # resolution
guess = np.linspace(joint_start, joint_target, n_grid)
# create trajopt instance
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
joint_start,
joint_target,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
# create some stuff for visualization
config = np.array(robot.getConfig())
def update_config(q):
config[link_index] = q
def get_config(q):
cq = copy.copy(config)
cq[link_index] = q
return cq
rst = trajopt.optimize(guess)
sol = rst['sol']
vis.add('world', world)
for i in range(1, n_grid -
2): # set to -2 so the robot stops there with collision
vis.add('ghost%d' % i, list(get_config(sol[i])))
vis.setColor('ghost%d' % i, 0, 1, 0, 0.5)
# dists = compute_distance(world, robot, link_index, obs_index)
# print(dists)
update_config(sol[-1])
vis.spin(float('inf'))
def visualize_in_klampt(self):
from klampt import vis
height = 0.25
for xyz in self.scatter:
name = f"{xyz[0]}{xyz[1]}{xyz[2]}"
traj = trajectory.Trajectory(
milestones=[[xyz[0], xyz[1], xyz[2] +
0.001], [xyz[0], xyz[1], xyz[2] + height]])
vis.add(name, traj)
vis.hideLabel(name)
for i in range(len(self.scatter) - 4, len(self.scatter)):
xyz = self.scatter[i]
name = f"{xyz[0]}{xyz[1]}{xyz[2]}_2"
traj = trajectory.Trajectory(
milestones=[[xyz[0], xyz[1], xyz[2] +
0.001], [xyz[0], xyz[1], xyz[2] + 1]])
vis.add(name, traj)
vis.setColor(name, 1, 0, 0, 1)
def ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj):
StartTime = ImpulseObj[0]
EndTime = ImpulseObj[1]
ImpulForce = ImpulseObj[2]
DurationTime = EndTime - StartTime
OriPoint = SimRobot.link(19).getWorldPosition([0.0, 0.0, 0.0])
ImpulseScale = 0.001 * (SimulationTime - StartTime) / DurationTime
EndPoint = OriPoint[:]
EndPoint[0] += ImpulForce[0] * ImpulseScale
EndPoint[1] += ImpulForce[1] * ImpulseScale
EndPoint[2] += ImpulForce[2] * ImpulseScale
if SimulationTime < StartTime:
pass
elif SimulationTime > EndTime:
vis.hide("Impulse", True)
else:
vis.add("Impulse", Trajectory([0, 1], [OriPoint, EndPoint]))
vis.hideLabel("Impulse", True)
vis.setColor("Impulse", 235.0 / 255.0, 52.0 / 255.0, 52.0 / 255.0, 1.0)
vis.setAttribute("Impulse", 'width', 5.0)
def checkCollision(self):
vis.lock()
## Checking collision
collisionFlag = False
for iR in range(self.n):
collRT0 = self.collisionChecker.robotTerrainCollisions(
self.world.robot(iR), self.world.terrain(0))
for i, j in collRT0:
collisionFlag = True
strng = "Robot collides with " + j.getName()
if self.showVis:
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
for iR in range(self.n):
collRT2 = self.collisionChecker.robotObjectCollisions(
self.world.robot(iR))
for i, j in collRT2:
collisionFlag = True
strng = self.world.robot(
iR).getName() + " collides with " + j.getName()
if self.showVis:
print(strng)
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
collRT3 = self.collisionChecker.robotSelfCollisions()
for i, j in collRT3:
collisionFlag = True
strng = i.getName() + " collides with " + j.getName()
if self.showVis:
vis.addText("textCol", strng)
vis.setColor("textCol", 0.8500, 0.3250, 0.0980)
vis.unlock()
return collisionFlag
break
if not collisionFlag:
if self.showVis:
vis.addText("textCol", "No collision")
vis.setColor("textCol", 0.4660, 0.6740, 0.1880)
vis.unlock()
return collisionFlag
def WeightedContactDataPlot(vis, StepNo, LimbNo, OptimalContact_data,
OptimalContactWeights_data):
scale = 1.0
for i in range(OptimalContact_data.size / 3):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = OptimalContact_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
point_end = [0.0, 0.0, 0.0]
ReachableContactWeight_i = OptimalContactWeights_data[i]
point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0]
point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1]
point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2]
ContactName = "Stage" + str(StepNo) + "LinkNo" + str(
LimbNo) + "Point:" + str(i)
print i
vis.add(ContactName, Trajectory([0, 1], [point_start, point_end]))
vis.hideLabel(ContactName, True)
vis.setColor(ContactName, 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute(ContactName, 'width', 5.0)
