def getJacobian(self, q):
self.robot.setConfig(q)
J = None
if self.taskType == 'po':
J = self.link.getJacobian(self.localPosition)
elif self.taskType == 'position':
J = self.link.getPositionJacobian(self.localPosition)
elif self.taskType == 'orientation':
J = self.link.getOrientationJacobian()
else:
raise ValueError("Invalid taskType " + self.taskType)
J = np.array(J)
#check if relative transform task, modify Jacobian accordingly
if self.baseLinkNo >= 0:
T = self.link.getTransform()
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
pb = se3.apply(Tbinv, se3.apply(T, self.localPosition))
if self.taskType == 'po':
Jb = self.baseLink.getJacobian(pb)
elif self.taskType == 'position':
Jb = self.baseLink.getPositionJacobian(pb)
elif self.taskType == 'orientation':
Jb = self.baseLink.getOrientationJacobian()
Jb = np.array(Jb)
#subtract out jacobian w.r.t. baseLink
J -= Jb
if self.activeDofs != None:
J = select_cols(J, self.activeDofs)
return J
def callback(robot_controller=robot_controller,
drawing_controller=drawing_controller,
storage=[sim_world, planning_world, sim, controller_vis]):
start_clock = time.time()
dt = 1.0 / robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
if robot_controller.status() == 'ok':
drawing_controller.advance(dt)
vis.addText("Status", drawing_controller.state, (10, 20))
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(
robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee = sim_robot.link(ee_link).getTransform()
vis.add(
"pen axis",
Trajectory([0, 1], [
se3.apply(Tee, ee_local_pos),
se3.apply(Tee,
vectorops.madd(ee_local_pos, ee_local_axis, lifth))
]),
color=(0, 1, 0, 1))
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0, dt - duration))
def set_path(self):
"""
:return: config_list: {list} -- the list of suturing path in robot configuration space
"""
config_list = []
localpos = [[0.0, 0.0, 0.0],
[-self.radius + self.radius * np.cos((120 * np.pi) / 180.),
-self.radius * np.sin((120 * np.pi) / 180.), 0.0],
[-self.radius + self.radius * np.cos((240 * np.pi) / 180.),
-self.radius * np.sin((240 * np.pi) / 180.), 0.0]]
center = [(self.dim_arr[0][0] * 1e-3) -
((self.suture_center[0, 0] / self.dim_arr[1][0]) * self.dim_arr[0][0] * 1e-3),
(self.suture_center[1, 0] / self.dim_arr[1][1]) * self.dim_arr[0][1] * 1e-3]
print("The center is: ", (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-3)
for i in range(0, self.rotation * 10):
current_angle = i * 0.1
pos_x_0 = center[0] - self.radius * np.cos((current_angle * np.pi) / 180.)
pos_y_0 = center[1] + self.radius * np.sin((current_angle * np.pi) / 180.)
pos_x_1 = center[0] - self.radius * np.cos(((current_angle - 120) * np.pi) / 180.)
pos_y_1 = center[1] + self.radius * np.sin(((current_angle - 120) * np.pi) / 180.)
pos_x_2 = center[0] - self.radius * np.cos(((current_angle - 240) * np.pi) / 180.)
pos_y_2 = center[1] + self.radius * np.sin(((current_angle - 240) * np.pi) / 180.)
world_pos = [se3.apply(self.Toct, [pos_x_0, pos_y_0, (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-2]),
se3.apply(self.Toct, [pos_x_1, pos_y_1, (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-2]),
se3.apply(self.Toct, [pos_x_2, pos_y_2, (self.suture_center[2, 0]/self.dim_arr[1][2]) * 1e-2])]
q = solve_ik(self.robot.link('tool0'), localpos, world_pos)
config_list.append(q)
return config_list
def getJacobian(self, q):
self.robot.setConfig(q)
J = None
if self.taskType == 'po':
J = self.link.getJacobian(self.localPosition)
elif self.taskType == 'position':
J = self.link.getPositionJacobian(self.localPosition)
elif self.taskType == 'orientation':
J = self.link.getOrientationJacobian()
else:
raise ValueError("Invalid taskType "+self.taskType)
J = np.array(J)
#check if relative transform task, modify Jacobian accordingly
if self.baseLinkNo >= 0:
T = self.link.getTransform()
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
pb = se3.apply(Tbinv,se3.apply(T,self.localPosition))
if self.taskType == 'po':
Jb = self.baseLink.getJacobian(pb)
elif self.taskType == 'position':
Jb = self.baseLink.getPositionJacobian(pb)
elif self.taskType == 'orientation':
Jb = self.baseLink.getOrientationJacobian()
Jb = np.array(Jb)
#subtract out jacobian w.r.t. baseLink
J -= Jb
if self.activeDofs!=None:
J = select_cols(J,self.activeDofs)
return J
def gen_est_config_list(robot, Clink_set, origin_config):
"""
:param robot: {Klampt.RobotModel} -- the robot model
:param Clink_set: {list} -- the strandard robot configuration list
:param origin_config: {list} -- the actual original start configuration
:return: est_config_list: {list} -- the estimated configuration list
"""
est_config_list = []
est_config_list.append(origin_config)
robot.setConfig(origin_config)
origin_trans = robot.link('link_6').getTransform()
robot.setConfig(Clink_set[0])
mother_trans = robot.link('link_6').getTransform()
diff_trans = se3.mul(origin_trans, se3.inv(mother_trans))
local_pos = [[1, 0.0, 0.0], [0.0, 1, 0.0], [0.0, 0.0, 1]]
for i in range(1, len(Clink_set)):
robot.setConfig(Clink_set[i])
child_trans = se3.mul(diff_trans, robot.link('link_6').getTransform())
relative_trans = se3.mul(child_trans, se3.inv(origin_trans))
robot.setConfig(origin_config)
world_pos = [
se3.apply(relative_trans,
robot.link('link_6').getWorldPosition(local_pos[0])),
se3.apply(relative_trans,
robot.link('link_6').getWorldPosition(local_pos[1])),
se3.apply(relative_trans,
robot.link('link_6').getWorldPosition(local_pos[2]))
]
q = solve_ik(robot.link('link_6'), local_pos, world_pos, i)
est_config_list.append(q)
return est_config_list
def apply_tendon_forces(self,i,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 10000.0
b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3))
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p0w = se3.apply(b1.getTransform(),self.tendon0_local)
p1w = se3.apply(b1.getTransform(),self.tendon1_local)
p2w = se3.apply(b2.getTransform(),self.tendon2_local)
d = vectorops.distance(p1w,p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w,p1w))
f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
#print d,rest_length
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
f = min(f,100)
#tendon routing force
straight = vectorops.unit(vectorops.sub(p2w,p0w))
pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
cosangle = vectorops.dot(straight,tangential_axis)
#print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
self.forces[i][2] = vectorops.mul(direction,f)
else:
self.forces[i] = [None,None,None]
return
def drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes if self.baseLinkNo >= 0: Tb = self.baseLink.getTransform() if self.taskType == "position": x = se3.apply(Tb,x) xdes = se3.apply(Tb,xdes) elif self.taskType == "po": x = se3.mul(Tb,x) xdes = se3.mul(Tb,xdes) glPointSize(6) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) if self.taskType == "position": glColor3f(1,0,0) #red glVertex3fv(xdes) glColor3f(1,0.5,0) #orange glVertex3fv(x) elif self.taskType == "po": glColor3f(1,0,0) #red glVertex3fv(xdes[1]) glColor3f(1,0.5,0) #orange glVertex3fv(x[1]) glEnd()
def display(self):
#draw points on the robot
lh = Hand('l')
rh = Hand('r')
glDisable(GL_LIGHTING)
glPointSize(5.0)
glDisable(GL_DEPTH_TEST)
glBegin(GL_POINTS)
glColor3f(0, 1, 0)
glVertex3fv(
se3.apply(
self.world.robot(0).link(lh.link).getTransform(),
lh.localPosition1))
glVertex3fv(
se3.apply(
self.world.robot(0).link(rh.link).getTransform(),
rh.localPosition1))
glColor3f(0, 0, 1)
glVertex3fv(
se3.apply(
self.world.robot(0).link(lh.link).getTransform(),
lh.localPosition2))
glVertex3fv(
se3.apply(
self.world.robot(0).link(rh.link).getTransform(),
rh.localPosition2))
glColor3f(1, 0, 0)
glVertex3fv(self.world.rigidObject(0).getTransform()[1])
glEnd()
glEnable(GL_DEPTH_TEST)
def drawGL(self, q):
x = self.getSensedValue(q)
xdes = self.xdes
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
if self.taskType == "position":
x = se3.apply(Tb, x)
xdes = se3.apply(Tb, xdes)
elif self.taskType == "po":
x = se3.mul(Tb, x)
xdes = se3.mul(Tb, xdes)
glPointSize(6)
glEnable(GL_POINT_SMOOTH)
glBegin(GL_POINTS)
if self.taskType == "position":
glColor3f(1, 0, 0) #red
glVertex3fv(xdes)
glColor3f(1, 0.5, 0) #orange
glVertex3fv(x)
elif self.taskType == "po":
glColor3f(1, 0, 0) #red
glVertex3fv(xdes[1])
glColor3f(1, 0.5, 0) #orange
glVertex3fv(x[1])
glEnd()
def display(self):
if self.running:
self.world.drawGL()
w_T_o = np.array(se3.homogeneous(self.obj.getTransform()))
for pose in self.poses:
w_T_p_des = w_T_o.dot(pose)
w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des)
draw_GL_frame(w_T_p__des_se3, color=(0.5,0.5,0.5))
if self.curr_pose is not None:
w_T_p_des = w_T_o.dot(self.curr_pose)
w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des)
draw_GL_frame(w_T_p__des_se3)
hand_xform = get_moving_base_xform(self.robot)
w_T_p = np.array(se3.homogeneous(hand_xform)).dot(self.h_T_p)
w_T_p_se3 = se3.from_homogeneous(w_T_p)
draw_GL_frame(w_T_p_se3)
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glEnable(GL_POINT_SMOOTH)
glColor3f(1,1,0)
glLineWidth(1.0)
glPointSize(5.0)
forceLen = 0.01 # scale of forces
if self.sim is not None and self.obj is not None and self.robot is not None:
c_p, c_f = getObjectPhalanxMeanContactPoint(self.sim, self.obj, self.robot, self.links_to_check)
n_c_p = countContactPoints(c_p)
if countContactPoints(c_p) > 0:
glBegin(GL_POINTS)
for i in range(len(c_p)/3):
o_c_p_i = c_p[3*i:3*i+3]
if np.all([False if math.isnan(p) else True for p in o_c_p_i]):
w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i)
glVertex3f(*w_c_p_i)
glEnd()
glBegin(GL_LINES)
for i in range(len(c_p)/3):
o_c_p_i = c_p[3 * i:3 * i + 3]
o_c_f_i = c_f[6 * i:6 * i + 3]
if np.all([False if math.isnan(f) else True for f in o_c_f_i]):
if np.all([False if math.isnan(p) else True for p in o_c_p_i]):
w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i)
w_c_f_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_f_i)
glVertex3f(*w_c_p_i)
glVertex3f(*vectorops.madd(w_c_p_i, w_c_f_i, forceLen))
glEnd()
def cal_est_config_list(robot, serial_num):
est_config_list = []
local_pos = [[1, 0.0, 0.0], [0.0, 1, 0.0], [0.0, 0.0, 1]]
Ticp_list = np.load("../../data/suture_experiment/standard_trans_list/trans_array.npy")
est_calibration = np.load("../../data/suture_experiment/calibration_result_files/" +
serial_num + "B/calibration_result.npy")
Toct = (so3.from_rpy(est_calibration[0:3]), est_calibration[3:6])
Tneedle = (so3.from_rpy(est_calibration[6:9]), est_calibration[9:12])
for i in range(0, 9):
Tee = se3.mul(se3.mul(Toct, se3.inv(Ticp_list[i])), se3.inv(Tneedle))
world_pos = [se3.apply(Tee, local_pos[0]),
se3.apply(Tee, local_pos[1]),
se3.apply(Tee, local_pos[2])]
q = solve_ik(robot.link('link_6'), local_pos, world_pos, i)
est_config_list.append(q)
return est_config_list
def transform_point_cloud(pc,
T,
point_channels=[0, 1, 2],
normal_channels=[6, 7, 8]):
"""Given a point cloud `pc` and a transform T, apply the transform
to the point cloud (in place).
Args:
pc (np.ndarray): an N x M numpy array, with N points and M
channels.
T (klampt se3 element): a Klamp't se3 element representing
the transform to apply.
point_channels (list of 3 ints): The channel indices (columns)
in pc corresponding to the point data.
normal_channels (list of 3 ints): The channels indices(columns)
in pc corresponding to the normal data. If this is None
or an index is >= M, just ignore.
"""
N, M = pc.shape
assert len(point_channels) == 3
for i in point_channels:
assert i < M, "Invalid point_channel"
for i in range(N):
point_data = pc[i, :]
point = point_data[point_channels]
point_data[point_channels] = se3.apply(T, point)
if normal_channels is not None and normal_channels[0] < M:
for i in normal_channels:
assert i < M, "Invalid normal_channel"
for i in range(N):
point_data = pc[i, :]
point = point_data[normal_channels]
point_data[normal_channels] = so3.apply(T[0], point)
def draw_part_circle_3d(radius, center, start_Z, rot_angle, start_angle, rot_x,
pix_x, pix_h, pix_z):
"""
:param radius: {float} -- the radius of suture path
:param center: {list} -- the suture center in the OCT frame
:param start_Z: {float} -- the suture path Z axis value in the OCT frame
:param rot_angle: {float} -- the rotation angle of the suture path
:param start_angle: {float} -- the start suture angle
:param rot_x: {float} -- the suture path rotation on the OCT frame X axis
:param pix_x: {float} -- the length in pixels x length
:param pix_h: {float} -- the length in pixels y length
:param pix_z: {float} -- the length in pixels z length
:return: suture_path: {list} -- the suture milestones in the world frame
"""
suture_path = []
rot_mat = (so3.from_rpy([rot_x, 0.0, 0.0]), [0.0] * 3)
for i in range(0, rot_angle * 10):
current_angle = start_angle + i * 0.1
pos_x_0 = center[0] - radius * np.cos((current_angle * np.pi) / 180.)
pos_y_0 = center[1] + radius * np.sin((current_angle * np.pi) / 180.)
milestone = np.divide(se3.apply(rot_mat, [pos_x_0, pos_y_0, start_Z]),
[pix_x, pix_h, pix_z])
suture_path.append(milestone)
return suture_path
def callback(robot_controller=robot_controller,drawing_controller=drawing_controller,trace=trace,
storage=[sim_world,planning_world,sim,controller_vis]):
start_clock = time.time()
dt = 1.0/robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
drawing_controller.advance(dt)
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee=sim_robot.link(ee_link).getTransform()
wp = se3.apply(Tee,ee_local_pos)
if len(trace.milestones) == 0 or vectorops.distance(wp,trace.milestones[-1]) > 0.01:
trace.milestones.append(wp)
trace.times.append(0)
if len(trace.milestones)==2:
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
if len(trace.milestones) > 200:
trace.milestones = trace.milestones[-100:]
trace.times = trace.times[-100:]
if len(trace.milestones)>2:
if _backend=='IPython':
vis.remove("trace")
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
else:
vis.dirty("trace")
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0,dt-duration))
def drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes #invert the transformations from task to world coordinates if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() xdes = se3.apply(Tb,self.xdes) x = se3.apply(Tb,x) glPointSize(10) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) glColor3f(0,1,0) #green glVertex3fv(xdes) glColor3f(0,1,1) #cyan glVertex3fv(x) glEnd()
def addForceSpring(self,obj,worldpt):
self.sim.updateWorld()
self.forceObject = obj
Ro,to = obj.getTransform()
self.forceAnchorPoint = worldpt
self.forceLocalPoint = se3.apply(se3.inv((Ro,to)),self.forceAnchorPoint)
self.sim.addHook(obj,lambda obj:self.simulateForceSpring())
def df_dz(self, x, y, z):
n = len(y)
m = len(z) // n
result = np.zeros((6, len(z)))
T = self.env_geom.getTransform()
CoM = se3.apply(T, self.env_obj.getMass().getCom())
for i in range(n):
point = y[i][1]
Normal_normal = normal(self.env_geom, point)
n1 = so3.canonical(Normal_normal)[3:6]
n2 = so3.canonical(Normal_normal)[6:9]
Normal_friction = []
for j in range(6):
n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin(
(math.pi / 3) * j) * np.array(n2)).tolist()
Normal_friction.append(vectorops.unit(n_tmp))
Cross_Normal = list(
vectorops.cross(vectorops.sub(point, CoM), Normal_normal))
Cross_Normal_friction = []
for j in range(6):
cross_Normal_v = list(
vectorops.cross(vectorops.sub(point, CoM),
Normal_friction[j]))
Cross_Normal_friction.append(cross_Normal_v)
result[0:3, i * m:(i + 1) * m - 1] = np.asarray(Normal_friction).T
result[0:3, (i + 1) * m - 1:(i + 1) *
m] = np.asarray(Normal_normal).reshape((3, 1))
result[3:6,
i * m:(i + 1) * m - 1] = np.asarray(Cross_Normal_friction).T
result[3:6, (i + 1) * m - 1:(i + 1) *
m] = np.asarray(Cross_Normal).reshape((3, 1))
return result
def drawGL(self, q):
x = self.getSensedValue(q)
xdes = self.xdes
#invert the transformations from task to world coordinates
if self.baseLinkNo >= 0:
Tb = self.robot.link(self.baseLinkNo).getTransform()
xdes = se3.apply(Tb, self.xdes)
x = se3.apply(Tb, x)
glPointSize(10)
glEnable(GL_POINT_SMOOTH)
glBegin(GL_POINTS)
glColor3f(0, 1, 0) #green
glVertex3fv(xdes)
glColor3f(0, 1, 1) #cyan
glVertex3fv(x)
glEnd()
def simulateForceSpring(self, kP=10.0):
if not self.forceApplicationMode: return
self.sim.updateWorld()
body = self.sim.body(self.forceObject)
T = self.forceObject.getTransform()
wp = se3.apply(T, self.forceLocalPoint)
f = vectorops.mul(vectorops.sub(self.forceAnchorPoint, wp), kP)
body.applyForceAtLocalPoint(f, self.forceLocalPoint)
def callback(self, data):
self.marker = data
self.marker_pos[0] = self.marker.pose.position.x
self.marker_pos[1] = self.marker.pose.position.y
self.marker_pos[2] = self.marker.pose.position.z
#=== transform to world coordinate
marker = se3.apply(kinect_frame_to_pedestal, tuple(self.marker_pos))
self.marker_pos_tf = list(marker)
self.marker_pos_tf[2] = self.marker_pos_tf[2] + self.base_height
def apply_tendon_forces(self, i, link1, link2, rest_length):
tendon_c2 = 30000.0
tendon_c1 = 10000.0
b0 = self.sim.body(
self.model.robot.link(self.model.proximal_links[0] - 3))
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p0w = se3.apply(b1.getTransform(), self.tendon0_local)
p1w = se3.apply(b1.getTransform(), self.tendon1_local)
p2w = se3.apply(b2.getTransform(), self.tendon2_local)
d = vectorops.distance(p1w, p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w, p1w))
f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d -
rest_length)
#print d,rest_length
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
f = min(f, 100)
#tendon routing force
straight = vectorops.unit(vectorops.sub(p2w, p0w))
pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w))
pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0))
tangential_axis = vectorops.cross(pulley_axis, pulley_direction)
cosangle = vectorops.dot(straight, tangential_axis)
#print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1])
b0.applyForceAtLocalPoint(
vectorops.mul(base_direction, -f),
vectorops.madd(p0w, base_direction, 0.04))
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, cosangle * f),
self.tendon1_local)
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, -cosangle * f),
self.tendon0_local)
b2.applyForceAtLocalPoint(vectorops.mul(direction, -f),
self.tendon2_local)
self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f)
self.forces[i][2] = vectorops.mul(direction, f)
else:
self.forces[i] = [None, None, None]
return
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv,T)
if self.taskType == 'po':
x = (T[0],se3.apply(T,self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T,self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType "+self.taskType)
return x
def getSensedValue(self, q): """Returns CoM position """ self.robot.setConfig(q) com = self.robot.getCom() if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() Tbinv = se3.inv(Tb) com = se3.apply(Tbinv,com) return com
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv, T)
if self.taskType == 'po':
x = (T[0], se3.apply(T, self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T, self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType " + self.taskType)
return x
def getSensedValue(self, q):
"""Returns CoM position
"""
self.robot.setConfig(q)
com = self.robot.getCom()
if self.baseLinkNo >= 0:
Tb = self.robot.link(self.baseLinkNo).getTransform()
Tbinv = se3.inv(Tb)
com = se3.apply(Tbinv, com)
return com
def statesCollection(self, each_blob):
'''
input: blob object
output: states and covariance(None)
Use invh to caculate the position of the ball, the vels are assumed to be zeros
'''
cur_obs = [each_blob.x, each_blob.y, each_blob.w, each_blob.h]
cur_p_c = self.hInvPosSolver(cur_obs, BALL_RADIUS, self.w / 2.0,
self.h / 2.0)
cur_states = se3.apply(self.Tsensor, cur_p_c.T.tolist()[0]) + [0, 0, 0]
cur_cov = None
return cur_states, cur_cov
def display(self):
GLSimulationPlugin.display(self)
#draw force springs if using
if self.forceApplicationMode:
self.sim.updateWorld()
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glColor3f(1,0.5,0)
glLineWidth(3.0)
glBegin(GL_LINES)
glVertex3fv(self.forceAnchorPoint)
glVertex3fv(se3.apply(self.forceObject.getTransform(),self.forceLocalPoint))
glEnd()
glLineWidth(1.0)
glEnable(GL_DEPTH_TEST)
def getJacobian(self, q): """Returns axis-weighted CoM Jacobian by averaging mass-weighted Jacobian of each link. """ self.robot.setConfig(q) numLinks = self.robot.numLinks() Jcom = np.array(self.robot.getComJacobian()) #if relative positioning task, subtract out COM jacobian w.r.t. base if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() pb = se3.apply(se3.inv(Tb),self.robot.getCom()) Jb = np.array(self.robot.link(self.baseLinkNo).getPositionJacobian(pb)) Jcom -= Jb if self.activeDofs != None: Jcom = select_cols(Jcom,self.activeDofs) return Jcom
def getJacobian(self, q): """Returns axis-weighted CoM Jacobian by averaging mass-weighted Jacobian of each link. """ self.robot.setConfig(q) numLinks = self.robot.numLinks() Jcom = np.array(self.robot.getComJacobian()) #if relative positioning task, subtract out COM jacobian w.r.t. base if self.baseLinkNo >= 0: Tb = self.robot.link(self.baseLinkNo).getTransform() pb = se3.apply(se3.inv(Tb),self.robot.getCom()) Jb = np.array(self.robot.link(self.baseLinkNo).getPositionJacobian(pb)) Jcom -= Jb if self.activeDofs != None: Jcom = select_cols(Jcom,self.activeDofs) return Jcom
def statesInitialization(self, each_blob, pre_xyz, num_pre_points):
'''
input: blob object from observation, previous collected positions, number of points for line fitting
output: position by hInvPosSolver, and vels approximated by line fitting
'''
cur_obs = [each_blob.x, each_blob.y, each_blob.w, each_blob.h]
cur_p_c = self.hInvPosSolver(cur_obs, BALL_RADIUS, self.w / 2.0,
self.h / 2.0)
cur_p_w = se3.apply(self.Tsensor, cur_p_c.T.tolist()[0])
cur_p_vel = self.velEst(pre_xyz, self.dt, num_pre_points,
num_pre_points)
cur_states = cur_p_w + cur_p_vel
cur_cov = np.vstack((np.zeros(
(3, 6)), np.hstack((np.zeros(
(3, 3)), np.eye(3) * (0.1 / 2.33)**2))))
return cur_states, cur_cov
def generate(self,camera_xform,camera_intrinsics,color_image,depth_image,k='auto'):
"""Returns a list of k AntipodalGrasps in world space according
to the given color and depth images. Returns a list of k scores
as well.
"""
#TODO: implement me for Problem3B
#tip: make your life easy and call code from problem2.py directly
pixel = np.unravel_index(depth_image.argmin(), depth_image.shape)
depth = depth_image[pixel]
fx,fy,cx,cy = camera_intrinsics['fx'],camera_intrinsics['fy'],camera_intrinsics['cx'],camera_intrinsics['cy']
y,x = pixel
Z = depth
X = (x - cx)/fx*Z
Y = (y - cy)/fy*Z
grasp_closest = AntipodalGrasp(se3.apply(camera_xform,[X,Y,Z]),[1,0,0])
score = 1.0
return [grasp_closest],[score]
def statesEstimation(self, each_blob, previous_estimate,
previous_covariance):
'''
input: previous_estimate(np.matrix)
output: states and covariance estimated by kalman filter
'''
pre_pos_w = previous_estimate[:3, 0].T.tolist()[0]
previous_p_c = se3.apply(se3.inv(self.Tsensor), pre_pos_w)
cur_obs = np.matrix(
[each_blob.x, each_blob.y, each_blob.w, each_blob.h]).T
cur_H = self.calJaccobian(previous_p_c, BALL_RADIUS)
cur_J = self.hTransfer(previous_p_c, BALL_RADIUS, 160,
120) - cur_H * previous_estimate
updated_states, cur_cov = kalman_filter_update(
previous_estimate, previous_covariance, self.F, self.g,
self.dyn_noise, cur_H, cur_J, self.obs_noise, cur_obs)
cur_states = updated_states.T.tolist()[0]
return cur_states, cur_cov
def next(self):
"""Returns the next Grasp from the database."""
if self.options is None:
return None
if self.index >= len(self.options):
self.options = None
return None
c, n, score = self.options(self.index)
self.index += 1
cworld = se3.apply(self.pc_xform, c)
nworld = so3.apply(self.pc_xform[0], n)
objective = IKObjective()
objective.setLinks(self.gripper.link)
objective.setFixedPoint(self.gripper.center, cworld)
objective.setAxialRotConstraint(self.gripper.primary_axis,
vectorops.mul(nworld, -1))
return Grasp(objective, score=score)
def compute_bb_tight(geom, Rt=None):
if geom.type() == "Group":
return geom.getBB()
#Rtc=geom.getCurrentTransform() if Rt is None else se3.mul(Rt,geom.getCurrentTransform())
#for i in range(geom.numElements()):
# e=geom.getElement(i)
# bb=union_bb(bb,compute_bb_tight(e,Rtc))
#return bb
elif geom.type() == "TriangleMesh":
bb = empty_bb(3)
m = geom.getTriangleMesh()
for v in range(len(m.vertices) // 3):
vert = [m.vertices[v * 3 + d] for d in range(3)]
if Rt is not None:
vert = se3.apply(Rt, vert)
bb = union_bb(bb, vert)
return bb
else:
return None
def value(self, x, y, z):
result = np.zeros(6)
if len(y) == 0:
result[0:3] += self.mass * self.gravity_coefficient * np.asarray(
self.gravity_direction)
return result
n = len(y)
m = len(z) // n
assert m == 7
T = self.env_geom.getTransform()
CoM = se3.apply(T, self.env_obj.getMass().getCom())
for i in range(n):
point = y[i][1]
f_normal = z[m * (i + 1) - 1]
f_friction = z[m * i:m * (i + 1) - 1]
Normal_normal = normal(self.env_geom, point)
n1 = so3.canonical(Normal_normal)[3:6]
n2 = so3.canonical(Normal_normal)[6:9]
Normal_friction = []
for j in range(6):
n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin(
(math.pi / 3) * j) * np.array(n2)).tolist()
Normal_friction.append(vectorops.unit(n_tmp))
Cross_Normal = list(
vectorops.cross(vectorops.sub(point, CoM), Normal_normal))
Cross_Normal_friction = []
for j in range(6):
cross_Normal_v = list(
vectorops.cross(vectorops.sub(point, CoM),
Normal_friction[j]))
Cross_Normal_friction.append(cross_Normal_v)
result[0:3] += np.asarray(Normal_normal) * f_normal + np.asarray(
f_friction) @ np.asarray(Normal_friction)
result[3:6] += np.asarray(Cross_Normal) * f_normal + np.asarray(
f_friction) @ np.asarray(Cross_Normal_friction)
result[0:3] += self.mass * self.gravity_coefficient * np.asarray(
self.gravity_direction)
return result
def show_workspace(grid):
vis.add("world",w)
res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid')
g_workspace = Geometry3D(res)
g_surface = g_workspace.convert('TriangleMesh',0.5)
if g_surface.numElements() != 0:
vis.add("reachable_boundary",g_surface,color=(1,1,0,0.5))
else:
print("Nothing reachable?")
Tee = robot.link(ee_link).getTransform()
gpen.setCurrentTransform(*Tee)
box = GeometricPrimitive()
box.setAABB(lower_corner,upper_corner)
gbox = Geometry3D(box)
#show this if you want to debug the size of the grid domain
#vis.add("box",gbox,color=(1,1,1,0.2))
vis.add("pen tip",se3.apply(Tee,ee_local_pos))
vis.loop()
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0, 1, 1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon0_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local))
glEnd()
glLineWidth(1)
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
fscale = 0.01
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
if self.forces[i][0] != None:
p = se3.apply(b1.getTransform(), self.tendon0_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][0], fscale))
if self.forces[i][1] != None:
p = se3.apply(b1.getTransform(), self.tendon1_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][1], fscale))
if self.forces[i][2] != None:
p = se3.apply(b2.getTransform(), self.tendon2_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p, self.forces[i][2], fscale))
glEnd()
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0,1,1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(),self.tendon0_local))
glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(),self.tendon2_local))
glEnd()
glLineWidth(1)
glColor3f(1,0.5,0)
glBegin(GL_LINES)
fscale = 0.01
for i in range(3):
b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
if self.forces[i][0] != None:
p = se3.apply(b1.getTransform(),self.tendon0_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,self.forces[i][0],fscale))
if self.forces[i][1] != None:
p = se3.apply(b1.getTransform(),self.tendon1_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,self.forces[i][1],fscale))
if self.forces[i][2] != None:
p = se3.apply(b2.getTransform(),self.tendon2_local)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,self.forces[i][2],fscale))
glEnd()
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
