def handle_cart_cmd(self, scaling): """""" try: # Get the current position of the hydra input_frame = fromTf(self.listener.lookupTransform( self.input_ref_frame_id, self.input_frame_id, rospy.Time(0))) # Get the current position of the end-effector tip_frame = fromTf(self.listener.lookupTransform( self.input_ref_frame_id, self.tip_link, rospy.Time(0))) # Capture the current position if we're starting to move if not self.deadman_engaged: self.deadman_engaged = True self.cmd_origin = input_frame self.tip_origin = tip_frame else: self.cart_scaling = scaling # Update commanded TF frame cmd_twist = kdl.diff(self.cmd_origin, input_frame) cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel #rospy.logwarn(cmd_twist) self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: rospy.logwarn(str(ex))
def handle_cart_cmd(self, msg): side = self.side try: # Get the current position of the hydra hydra_frame = fromTf(self.listener.lookupTransform( '/hydra_base', '/hydra_'+self.SIDE_STR[side]+'_grab', rospy.Time(0))) # Get the current position of the end-effector tip_frame = fromTf(self.listener.lookupTransform( '/world', self.tip_link, rospy.Time(0))) # Capture the current position if we're starting to move if not self.deadman_engaged: self.deadman_engaged = True self.cmd_origin = hydra_frame self.tip_origin = tip_frame else: self.deadman_max = max(self.deadman_max, msg.axes[self.DEADMAN[side]]) # Update commanded TF frame cmd_twist = kdl.diff(self.cmd_origin, hydra_frame) cmd_twist.vel = self.scale*self.deadman_max*cmd_twist.vel self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: rospy.logwarn(str(ex))
def updateCom(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights, m_id=0, pub_marker=None): diff_B = PyKDL.diff(T_B_O, T_B_O_2) up_v_B = PyKDL.Vector(0, 0, 1) n_v_B = diff_B.rot * up_v_B if pub_marker != None: m_id = pub_marker.publishVectorMarker( PyKDL.Vector(), diff_B.rot, m_id, r=0, g=1, b=0, frame="world", namespace="default", scale=0.01 ) m_id = pub_marker.publishVectorMarker( PyKDL.Vector(), n_v_B, m_id, r=1, g=1, b=1, frame="world", namespace="default", scale=0.01 ) n_O = PyKDL.Frame(T_B_O.Inverse().M) * n_v_B n_O_2 = PyKDL.Frame(T_B_O_2.Inverse().M) * n_v_B # get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2 c_dot_list = [] c_dot_list_2 = [] for c in contacts_O: dot = PyKDL.dot(c, n_O) c_dot_list.append(dot) dot_2 = PyKDL.dot(c, n_O_2) c_dot_list_2.append(dot_2) # get all com points that lies in the positive direction from given contact for contact_idx in range(0, len(c_dot_list)): for com_idx in range(0, len(com_pt)): c = com_pt[com_idx] dot = PyKDL.dot(c, n_O) dot_2 = PyKDL.dot(c, n_O_2) if dot > c_dot_list[contact_idx] and dot_2 > c_dot_list_2[contact_idx]: com_weights[com_idx] += 1 return m_id
def getCartImpWristTraj(self, js, goal_T_B_W): init_js = copy.deepcopy(js) init_T_B_W = self.fk_solver.calculateFk("right_arm_7_link", init_js) T_B_Wd = goal_T_B_W T_B_W_diff = PyKDL.diff(init_T_B_W, T_B_Wd, 1.0) steps = int(max(T_B_W_diff.vel.Norm() / 0.05, T_B_W_diff.rot.Norm() / (20.0 / 180.0 * math.pi))) if steps < 2: steps = 2 self.updateJointLimits(init_js) self.updateMimicJoints(init_js) q_list = [] for f in np.linspace(0.0, 1.0, steps): T_B_Wi = PyKDL.addDelta(init_T_B_W, T_B_W_diff, f) q_out = self.fk_solver.simulateTrajectory("right_arm_7_link", init_js, T_B_Wi) if q_out == None: # print "error: getCartImpWristTraj: ", str(f) return None q_list.append(q_out) for i in range(7): joint_name = self.fk_solver.ik_joint_state_name["right_arm_7_link"][i] init_js[joint_name] = q_out[i] return q_list
def updateMouthFrames(self, head_frame): mouth_frame = copy.deepcopy(head_frame) ## Position mouth_frame.p[2] -= self.head_z_offset ## Rotation rot = mouth_frame.M ## head_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]]) tx = PyKDL.Vector(1.0, 0.0, 0.0) ty = PyKDL.Vector(0.0, 1.0, 0.0) # Projection to xy plane px = PyKDL.dot(tx, rot.UnitZ()) py = PyKDL.dot(ty, rot.UnitZ()) mouth_y = rot.UnitY() mouth_z = PyKDL.Vector(px, py, 0.0) mouth_z.Normalize() mouth_x = mouth_y * mouth_z mouth_y = mouth_z * mouth_x mouth_rot = PyKDL.Rotation(mouth_x, mouth_y, mouth_z) mouth_frame.M = mouth_rot mouth_frame_off = head_frame.Inverse()*mouth_frame if self.mouth_frame_off == None: self.mouth_frame_off = mouth_frame_off else: self.mouth_frame_off.p = (self.mouth_frame_off.p + mouth_frame_off.p)/2.0 pre_quat = geometry_msgs.msg.Quaternion() pre_quat.x = self.mouth_frame_off.M.GetQuaternion()[0] pre_quat.y = self.mouth_frame_off.M.GetQuaternion()[1] pre_quat.z = self.mouth_frame_off.M.GetQuaternion()[2] pre_quat.w = self.mouth_frame_off.M.GetQuaternion()[3] cur_quat = geometry_msgs.msg.Quaternion() cur_quat.x = mouth_frame_off.M.GetQuaternion()[0] cur_quat.y = mouth_frame_off.M.GetQuaternion()[1] cur_quat.z = mouth_frame_off.M.GetQuaternion()[2] cur_quat.w = mouth_frame_off.M.GetQuaternion()[3] # check close quaternion and inverse if np.dot(self.mouth_frame_off.M.GetQuaternion(), mouth_frame_off.M.GetQuaternion()) < 0.0: cur_quat.x *= -1. cur_quat.y *= -1. cur_quat.z *= -1. cur_quat.w *= -1. quat = qt.slerp(pre_quat, cur_quat, 0.5) self.mouth_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
def updateBowlcenFrames(self, bowl_frame): bowl_cen_frame = copy.deepcopy(bowl_frame) ## Rotation rot = bowl_cen_frame.M ## bowl_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]]) tx = PyKDL.Vector(1.0, 0.0, 0.0) ty = PyKDL.Vector(0.0, 1.0, 0.0) # Projection to xy plane px = PyKDL.dot(tx, rot.UnitZ()) py = PyKDL.dot(ty, rot.UnitZ()) bowl_cen_y = rot.UnitY() bowl_cen_z = PyKDL.Vector(px, py, 0.0) bowl_cen_z.Normalize() bowl_cen_x = bowl_cen_y * bowl_cen_z bowl_cen_y = bowl_cen_z * bowl_cen_x bowl_cen_rot = PyKDL.Rotation(bowl_cen_x, bowl_cen_y, bowl_cen_z) bowl_cen_frame.M = bowl_cen_rot ## Position bowl_cen_frame.p[2] -= self.bowl_z_offset bowl_cen_frame.p += bowl_cen_z * self.bowl_forward_offset if bowl_cen_y[2] > bowl_cen_x[2]: # -90 x axis rotation bowl_cen_frame = bowl_cen_frame * self.x_neg90_frame else: # 90 y axis rotation bowl_cen_frame = bowl_cen_frame * self.y_90_frame bowl_cen_frame_off = bowl_frame.Inverse()*bowl_cen_frame if self.bowl_cen_frame_off == None: self.bowl_cen_frame_off = bowl_cen_frame_off else: self.bowl_cen_frame_off.p = (self.bowl_cen_frame_off.p + bowl_cen_frame_off.p)/2.0 pre_quat = geometry_msgs.msg.Quaternion() pre_quat.x = self.bowl_cen_frame_off.M.GetQuaternion()[0] pre_quat.y = self.bowl_cen_frame_off.M.GetQuaternion()[1] pre_quat.z = self.bowl_cen_frame_off.M.GetQuaternion()[2] pre_quat.w = self.bowl_cen_frame_off.M.GetQuaternion()[3] cur_quat = geometry_msgs.msg.Quaternion() cur_quat.x = bowl_cen_frame_off.M.GetQuaternion()[0] cur_quat.y = bowl_cen_frame_off.M.GetQuaternion()[1] cur_quat.z = bowl_cen_frame_off.M.GetQuaternion()[2] cur_quat.w = bowl_cen_frame_off.M.GetQuaternion()[3] quat = qt.slerp(pre_quat, cur_quat, 0.5) self.bowl_cen_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
def calculateWrenchesForTransportTask(self, ext_wrenches_W, transport_T_B_O): ext_wrenches_O = [] for i in range(len(transport_T_B_O)-1): diff_B_O = PyKDL.diff(transport_T_B_O[i], transport_T_B_O[i+1]) # simulate object motion and calculate expected wrenches for t in np.linspace(0.0, 1.0, 5): T_B_Osim = PyKDL.addDelta(transport_T_B_O[i], diff_B_O, t) T_Osim_B = T_B_Osim.Inverse() for ewr in ext_wrenches_W: ext_wrenches_O.append(PyKDL.Frame(T_Osim_B.M) * ewr) return ext_wrenches_O
def getGraspingAxis(bin_frame, obj_frame, object_name, simtrackUsed): ''' this function assumes everything is represented in the quaternions in the /base_link frame ''' if object_name.endswith('_scan'): object_name = object_name[:-5] dictObj = objDict() objSpec = dictObj.getEntry(object_name) F_bin_frame = posemath.fromTf(bin_frame) F_obj_frame = posemath.fromTf(obj_frame) objRed = F_obj_frame.M * kdl.Vector(1.0, 0.0, 0.0) objGreen = F_obj_frame.M * kdl.Vector(0.0, 1.0, 0.0) objBlue = F_obj_frame.M * kdl.Vector(0.0, 0.0, 1.0) binRed = F_bin_frame.M * kdl.Vector(1.0, 0.0, 0.0) binGreen = F_bin_frame.M * kdl.Vector(0.0, 1.0, 0.0) binBlue = F_bin_frame.M * kdl.Vector(0.0, 0.0, 1.0) rRProj = kdl.dot(objRed , binRed) gRProj = kdl.dot(objGreen, binRed) bRProj = kdl.dot(objBlue, binRed) tmpApproach1 = [abs(rRProj), abs(gRProj), abs(bRProj)] if simtrackUsed: for i in range(3): if i in objSpec.invalidApproachAxis: tmpApproach1[i] = 0 tmpApproach2 = [rRProj, gRProj, bRProj] axisApproach = tmpApproach1.index(max(tmpApproach1)) objAxes = [objRed, objGreen, objBlue] tmpGrasp1 = [] for i in range(3): if simtrackUsed: if i == axisApproach or i in objSpec.invalidGraspAxis: tmpGrasp1.append(0) continue tmpGrasp1.append(kdl.dot(objAxes[i], binBlue)) tmpGrasp2 = [abs(t) for t in tmpGrasp1] axisGrasp = tmpGrasp2.index(max(tmpGrasp2)) return axisApproach, tmpApproach2[axisApproach]/abs(tmpApproach2[axisApproach]), axisGrasp, tmpGrasp1[axisGrasp]/abs(tmpGrasp1[axisGrasp])
def distanceLinePoint(self, line, pt): a, b = line v = b - a ta = PyKDL.dot(v, a) tb = PyKDL.dot(v, b) tpt = PyKDL.dot(v, pt) if tpt <= ta: return (a-pt).Norm(), a, pt elif tpt >= tb: return (b-pt).Norm(), b, pt else: n = PyKDL.Vector(v.y(), -v.x(), v.z()) n.Normalize() diff = PyKDL.dot(n, a) - PyKDL.dot(n, pt) return abs(diff), pt + (diff * n), pt
def calc_R(xa, ya): ret = [] """ calculate the minimum distance of each contact point from jar surface pt """ n = PyKDL.Frame(PyKDL.Rotation.RotX(xa)) * PyKDL.Frame(PyKDL.Rotation.RotY(ya)) * PyKDL.Vector(0, 0, 1) for p in points: ret.append(PyKDL.dot(n, p)) return numpy.array(ret)
def _update(self): # Leave if ROS is not running or command is not valid if rospy.is_shutdown() or self._last_goal is None: return # Calculate the goal pose goal = self._get_goal() # End-effector's pose ee_pose = self._arm_interface.get_ee_pose_as_frame() # End-effector's velocity ee_vel = self._arm_interface.get_ee_vel_as_kdl_twist() # Calculate pose error error = PyKDL.diff(goal, ee_pose) # End-effector wrench to achieve target wrench = np.matrix(np.zeros(6)).T for i in range(len(wrench)): wrench[i] = -(self._Kp[i] * error[i] + self._Kd[i] * ee_vel[i]) # Compute jacobian transpose JT = self._arm_interface.jacobian_transpose() # Calculate the torques for the joints tau = JT * wrench # Store current pose target self._last_goal = goal self.publish_goal() self.publish_joint_efforts(tau)
def _standing_still_for_x_seconds(self, timeout): """Check whether the robot is standing still for X seconds :param timeout how many seconds must the robot be standing still before returning True :type timeout float :returns bool indicating whether the robot has been standing still for longer than timeout seconds""" current_frame = self._robot.base.get_location().frame now = rospy.Time.now() if not self._last_pose_stamped: self._last_pose_stamped = current_frame self._last_pose_stamped_time = now else: current_yaw = current_frame.M.GetRPY()[2] # Get the Yaw last_yaw = self._last_pose_stamped.M.GetRPY()[2] # Get the Yaw # Compare the pose with the last pose and update if difference is larger than x if kdl.diff(current_frame.p, self._last_pose_stamped.p).Norm() > 0.05 or abs(current_yaw - last_yaw) > 0.3: # Update the last pose self._last_pose_stamped = current_frame self._last_pose_stamped_time = rospy.Time.now() else: print "Robot dit not move for x seconds: %f"%(now - self._last_pose_stamped_time).to_sec() # Check whether we passed the timeout if (now - self._last_pose_stamped_time).to_sec() > timeout: return True return False
def calc_R(px, py, pz): R_7_M = PyKDL.Frame(rot_mx, PyKDL.Vector(px, py, pz)) ret = [] for idx in range(0,len(T_7bp_7i)): diff = PyKDL.diff( T_7bp_7i[idx] * R_7_M, R_7_M * T_Mbp_Mi[idx] ) ret.append( diff.vel.Norm() * weights_pos[idx] ) return ret
def get_frame_normal(door): """Get the normal of the door frame. @type door: door_msgs.msg.Door @rtype: kdl.Vector """ # normal on frame p12 = kdl.Vector( door.frame_p1.x - door.frame_p2.x, door.frame_p1.y - door.frame_p2.y, door.frame_p1.z - door.frame_p2.z) p12.Normalize() z_axis = kdl.Vector(0,0,1) normal = p12 * z_axis # make normal point in direction we travel through door direction = kdl.Vector( door.travel_dir.x, door.travel_dir.y, door.travel_dir.z) if kdl.dot(direction, normal) < 0: normal = normal * -1.0 return normal
def calc_R(rx, ry, rz): R_7_M = PyKDL.Frame(PyKDL.Rotation.EulerZYX(rx, ry, rz)) ret = [] for idx in range(0,len(T_7bo_7i)): diff = PyKDL.diff( T_7bo_7i[idx] * R_7_M, R_7_M * T_Mbo_Mi[idx] ) ret.append( diff.rot.Norm() * weights_ori[idx] ) return ret
def sampleOptimalZAngle(moveItInterface, R_bin_sample, R_eef_gripper_base, desiredZAxis, numAngles): ''' Find optimal angle for rotating a pose sample around Z-axis so that the gripper base's z axis aligns as much as possible with the desired z axis w.r.t the bin. @param R_bin_sample: rotation of the sample w.r.t. bin frame (kdl.Rotation) @param R_eef_gripper_base: rotation of the eef (tool-tip) w.r.t. gripper base frame (kdl.Rotation) @return: optimal angle (float) ''' z_bin_gripper_base_desired = kdl.Vector(*(desiredZAxis)) # evaluate for a series of rotations around z axis thetas_z = numpy.linspace(0, 2 * numpy.pi, numAngles) dot_products = numpy.array([]) for angle in thetas_z: moveItInterface.checkPreemption() R_bin_sample_rotated = R_bin_sample * kdl.Rotation.RotZ(angle) # resulting Z-axis of gripper_base z_bin_gripper_base = R_bin_sample_rotated * R_eef_gripper_base * kdl.Vector(0.0, 0.0, 1.0) dot_products = numpy.append(dot_products, kdl.dot(z_bin_gripper_base, z_bin_gripper_base_desired)) optimal_angle = thetas_z[dot_products.argmax()] return optimal_angle
def compute(self): vec1 = self.vec1.compute() vec2 = self.vec2.compute() denom = vec1.Norm() * vec2.Norm() if denom == 0: return 0 else: return kdl.dot(vec1, vec2) / denom
def compute(self): vec = self.vec.compute() ref = self.ref.compute() denom = ref.dir.Norm()**2 if denom == 0: res = kdl.Vector() else: res = ref.dir * (kdl.dot(ref.dir, vec.dir) / denom) return LocatedVector(vec.pos, res)
def determine_points_of_interest(self, center_frame, z_max, convex_hull): """ Determine candidates for place poses :param center_frame: kdl.Frame, center of the Entity to place on top of :param z_max: float, height of the entity to place on, w.r.t. the entity :param convex_hull: [kdl.Vector], convex hull of the entity :return: [FrameStamped] of candidates for placing """ points = [] if len(convex_hull) == 0: rospy.logerr('determine_points_of_interest: Empty convex hull') return [] # Convert convex hull to map frame ch = offsetConvexHull(convex_hull, center_frame) # Loop over hulls self.marker_array.markers = [] for i in xrange(len(ch)): j = (i + 1) % len(ch) dx = ch[j].x() - ch[i].x() dy = ch[j].y() - ch[i].y() length = kdl.diff(ch[j], ch[i]).Norm() d = self._edge_distance while d < (length - self._edge_distance): # Point on edge xs = ch[i].x() + d / length * dx ys = ch[i].y() + d / length * dy # Shift point inwards and fill message fs = kdl_frame_stamped_from_XYZRPY(x=xs - dy / length * self._edge_distance, y=ys + dx / length * self._edge_distance, z=center_frame.p.z() + z_max, frame_id="/map") # It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might # prevent the robot from hitting the cabinet edges # print "Length: {}, edge score: {}".format(length, min(d, length-d)) setattr(fs, 'edge_score', min(d, length-d)) points += [fs] self.marker_array.markers.append(self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z())) # ToDo: check if still within hull??? d += self._spacing self.marker_pub.publish(self.marker_array) return points
def axis_marker(tw, id = 0, ns = 'twist'): """ make a marker message showing the instantaneous rotation axis of a twist message""" t = kdl.Twist(kdl.Vector(tw.linear.x, tw.linear.y, tw.linear.z), kdl.Vector(tw.angular.x, tw.angular.y, tw.angular.z)) try: (x, rot) = listener.lookupTransform(target_frame, ref_frame, rospy.Time(0)) (trans, rotp) = listener.lookupTransform(target_frame, ref_point, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException): print 'tf exception!' return marker.create(id=id, ns=ns, action=Marker.DELETE) # put the twist in the right frame f = posemath.fromTf( (trans, rot) ) t = f*t direction = t.rot location = t.rot * t.vel / kdl.dot(t.rot, t.rot) lr = t.rot.Norm() lt = t.vel.Norm() m = marker.create(id=id, ns=ns, type=Marker.CYLINDER) if lr < 0.0001 and lt < 0.0001: return marker.create(id=id, ns=ns, action=Marker.DELETE) if lr < 0.001: # very short axis, assume linear movement location = kdl.Vector(0,0,0) direction = t.vel if lt < min_length: direction *= min_length / lt m.type = Marker.CUBE m = marker.align(m, location, location + direction, 0.02) elif lr < min_length: direction = direction / lr * min_length m = marker.align(m, location - direction, location + direction, 0.02) elif lr > max_length: direction = direction / lr * max_length m = marker.align(m, location - direction, location + direction, 0.02) else: #BAH! How do I make this better? m = marker.align(m, location - direction, location + direction, 0.02) m.header.frame_id = target_frame m.frame_locked = True if(use_colors): m.color = colors[id % len(colors)] else: m.color = ColorRGBA(0.3, 0.3, 0.3, 1) return m
def calc_R(rx, ry, rz): R_mean = PyKDL.Frame(PyKDL.Rotation.EulerZYX(rx, ry, rz)) diff = [] for r in R: diff.append(PyKDL.diff( R_mean, r )) ret = [] for idx in range(0, len(diff)): rot_err = diff[idx].rot.Norm() ret.append(rot_err * wg[idx] / wg_sum) return ret
def updateCom2(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights): diff = PyKDL.diff(T_B_O, T_B_O_2) up_v = PyKDL.Vector(0, 0, 1) n_v = diff.rot * up_v n_O = T_B_O.Inverse() * n_v n_O_2 = T_B_O_2.Inverse() * n_v # get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2 # get the minimum contact point for n_O min_c_dot = None for c in contacts_O: dot = PyKDL.dot(c, n_O) if min_c_dot == None or dot < min_c_dot: min_c_dot = dot # get all com points that lies in the positive direction from min_c_dot com_2_idx = [] for c_idx in range(0, len(com_pt)): c = com_pt[c_idx] dot = PyKDL.dot(c, n_O) if dot > min_c_dot: com_2_idx.append(c_idx) # get the minimum contact point for n_O min_c_dot = None for c in contacts_O: dot = PyKDL.dot(c, n_O_2) if min_c_dot == None or dot < min_c_dot: min_c_dot = dot # get all com points that lies in the positive direction from min_c_dot com_3_idx = [] for c_idx in com_2_idx: c = com_pt[c_idx] dot = PyKDL.dot(c, n_O_2) if dot > min_c_dot: com_3_idx.append(c_idx) for c_idx in range(0, len(com_pt)): com_weights[c_idx] -= 1 for c_idx in com_3_idx: com_weights[c_idx] += 2
def checkGoal(self, q): # interpolate trajectory (in the cartesian space) self.openrave.robot_rave.SetDOFValues(q, self.dof_indices) link_E = self.openrave.robot_rave.GetLink("right_HandPalmLink") T_World_E = conv.OpenraveToKDL(link_E.GetTransform()) T_B_E = self.openrave.T_World_Br.Inverse() * T_World_E for T_B_Eg in self.goals_T_B_E: diff = PyKDL.diff(T_B_Eg, T_B_E) if diff.vel.Norm() <= 0.02 and diff.rot.Norm() <= 10.0/180.0*math.pi: return True return False
def setParams(self, param): ''' 0-15: homogeneous goal matrix 16: slow down distance ''' gpos = matrix([param[0:4], param[4:8], param[8:12], param[12:16]]) self.gp = gpos[0:3, 3].T.A[0] self.dToAttractor = length(self.p - self.gp) self.slowDownDistance = param[16] for i in range(3): for j in range(3): self.gframe.M[i, j] = gpos[i, j] self.tw = kdl.diff(self.frame, self.gframe)
def within_tolerance(self, tip_pose, target_pose, scale=1.0): # compute cartesian command error tip_frame = fromTf(tip_pose) target_frame = fromTf(target_pose) twist_err = kdl.diff(tip_frame, target_frame) linear_err = twist_err.vel.Norm() angular_err = twist_err.rot.Norm() #print("linear: %g, angular %g" % (linear_err, angular_err)) # decide if planning is needed return linear_err < scale*self.linear_err_threshold and angular_err < scale*self.angular_err_threshold
def getMovementTime2(self, T_B_Wd1, T_B_Wd2, max_v_l = 0.1, max_v_r = 0.2): twist = PyKDL.diff(T_B_Wd1, T_B_Wd2, 1.0) v_l = twist.vel.Norm() v_r = twist.rot.Norm() print "v_l: %s v_r: %s"%(v_l, v_r) f_v_l = v_l/max_v_l f_v_r = v_r/max_v_r if f_v_l > f_v_r: duration = f_v_l else: duration = f_v_r if duration < 0.5: duration = 0.5 return duration
def markers(self): ''' get the markers for visualization ''' self._compute_lengths() frame = self._transform() mrks = [] pos = kdl.Vector(0,0,0) l_index = 0 for i,t in enumerate(self.jac): twist = frame*t lr = twist.rot.Norm() lv = twist.vel.Norm() if lr < self.eps and lv < self.eps: # null twist, make a delete marker mrks.append(marker.create(id=i, ns=self.name+str(i), action=Marker.DELETE)) continue if lr < self.eps: # pure translational twist location = pos direction = twist.vel / lv * self.lengths[l_index] m = marker.create(id=i, ns=self.name+str(i), type=Marker.CUBE) m = marker.align(m, location, location + direction, self.width) l_index += 1 frame.p = frame.p - direction # move target point to end of this 'axis' pos += direction # remember current end point else: # rotational twist location = twist.rot * twist.vel / kdl.dot(twist.rot, twist.rot) + pos if self.independent_directions: direction = twist.rot * self.rot_length / lr else: # take axis from interaction matrix and transform to location dir_H = self.H[i].rot - location*self.H[i].vel direction = dir_H * self.rot_length / dir_H.Norm() m = marker.create(id=i, ns=self.name+str(i), type=Marker.CYLINDER) m = marker.align(m, location - direction, location + direction, self.width) m.color = self._color(i) m.header.frame_id = self.target_frame mrks.append(m) return mrks
def __init__(self): self.frame=kdl.Frame() self.gframe=kdl.Frame() self.gp=array([0,0,0]) self.p=array([0,0,0]) self.dToAttractor=0 self.slowDownDistance=0.05 #normal value 0.03 self.tw=kdl.diff(self.frame,self.gframe) self.rot_factor=1.0 self.min_rot=0.09 self.stopDistance=0.005 self.r_stopDistance=0.03 self.slowDownTime=0.1 self.slowDowninitTime=time.time() self.time=time.time() self.filter=[1.0]*5 self.filterC=array([0.05,0.3,0.3,0.3,0.05])
def getVector(self,pos): ''' 0-15: position''' pos=matrix([pos[0:4],pos[4:8],pos[8:12],pos[12:16]]) self.p=pos[0:3,3].T.A[0] r=pos[0:3,0:3] #Cartesian vector self.dToAttractor=length(self.p-self.gp) if self.dToAttractor==0: P=array([0,0,0]) else: P=-(self.p-self.gp)/self.dToAttractor #todo: to improve stability we could decay near the attraction point #rotational twist for i in range(3): for j in range(3): self.frame.M[i,j]=r[i,j] self.tw=kdl.diff(self.frame,self.gframe) return concatenate((P,array([self.tw.rot[0],self.tw.rot[1],self.tw.rot[2]])),0)
def inertia(self, joint_values=None): inertia = PyKDL.JntSpaceInertiaMatrix(self._num_jnts) self._dyn_kdl.JntToMass(self.joints_to_kdl('positions', joint_values), inertia) return self.kdl_to_mat(inertia)
def setup_kdl_chain(self): for dh in self.dh_params: self.kine_chain.addSegment(PyKDL.Segment(PyKDL.Joint(PyKDL.Vector(), PyKDL.Vector(0, 0, dh[4]), PyKDL.Joint.RotAxis), PyKDL.Frame().DH(dh[0], dh[1], dh[2], dh[3])))
def TrotZ(theta): s = math.sin(theta) c = math.cos(theta) T = kdl.Frame(kdl.Rotation(c, -s, 0, s, c, 0, 0, 0, 1), kdl.Vector(0, 0, 0)) return T
#! /usr/bin/python import rospy import json import PyKDL as kdl import os from sensor_msgs.msg import * from geometry_msgs.msg import * from tf.transformations import * from visualization_msgs.msg import Marker kdlChain = kdl.Chain() frame = kdl.Frame() print("przed") print(kdlChain) print("po") with open(os.path.dirname(os.path.realpath(__file__)) + '/../dh.json', 'r') as file: params = json.loads(file.read()) def callback(data): kdlChain = kdl.Chain() frame = kdl.Frame() with open( os.path.dirname(os.path.realpath(__file__)) + '/../dh.json', 'r') as file: params = json.loads(file.read()) matrices = {} for key in sorted(params.keys()):
def run_jaw_move(self): print_id('starting jaw move') # try to open and close with the cartesian part of the arm in different modes print_id('close and open without other move command') input(" Press Enter to continue...") print_id('closing (1)') self.arm.jaw.close().wait() print_id('opening (2)') self.arm.jaw.open().wait() print_id('closing (3)') self.arm.jaw.close().wait() print_id('opening (4)') self.arm.jaw.open().wait() # try to open and close with a joint goal print_id('close and open with joint move command') input(" Press Enter to continue...") print_id('closing and moving up (1)') self.arm.jaw.close().wait(is_busy=True) self.arm.insert_jp(0.1).wait() print_id('opening and moving down (2)') self.arm.jaw.open().wait(is_busy=True) self.arm.insert_jp(0.15).wait() print_id('closing and moving up (3)') self.arm.jaw.close().wait(is_busy=True) self.arm.insert_jp(0.1).wait() print_id('opening and moving down (4)') self.arm.jaw.open().wait(is_busy=True) self.arm.insert_jp(0.15).wait() print_id('close and open with cartesian move command') input(" Press Enter to continue...") # try to open and close with a cartesian goal self.prepare_cartesian() initial_cartesian_position = PyKDL.Frame() initial_cartesian_position.p = self.arm.setpoint_cp().p initial_cartesian_position.M = self.arm.setpoint_cp().M goal = PyKDL.Frame() goal.p = self.arm.setpoint_cp().p goal.M = self.arm.setpoint_cp().M # motion parameters amplitude = 0.05 # 5 cm # first motion goal.p[0] = initial_cartesian_position.p[0] - amplitude goal.p[1] = initial_cartesian_position.p[1] print_id('closing and moving right (1)') self.arm.move_cp(goal).wait(is_busy=True) self.arm.jaw.close().wait() # second motion goal.p[0] = initial_cartesian_position.p[0] + amplitude goal.p[1] = initial_cartesian_position.p[1] print_id('opening and moving left (1)') self.arm.move_cp(goal).wait(is_busy=True) self.arm.jaw.open().wait() # back to starting point goal.p[0] = initial_cartesian_position.p[0] goal.p[1] = initial_cartesian_position.p[1] print_id('moving back (3)') self.arm.move_cp(goal).wait()
def convert_msg_point_to_kdl_vector(point): return PyKDL.Vector(point.x, point.y, point.z)
def gravity_compensation(self, world): m = world.get_object_mass(self._object_name) g = world.get_gravity() return kdl.Wrench(-m * g, kdl.Vector())
snake = u2c.URDFparser() snake.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) snake_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE) #joint info jointlist, names, q_max, q_min = snake.get_joint_info(root, tip) n_joints = snake.get_n_joints(root, tip) #declarations #kdl q_kdl = kdl.JntArray(n_joints) qdot_kdl = kdl.JntArray(n_joints) gravity_kdl = kdl.Vector() gravity_kdl[2] = -9.81 C_kdl = kdl.JntArray(n_joints) g_kdl = kdl.JntArray(n_joints) #u2c & pybullet q = [None]*n_joints qdot = [None]*n_joints zeros_pb = [None]*n_joints gravity_u2c = [0, 0, -9.81] g_sym = snake.get_gravity_rnea(root, tip, gravity_u2c) C_sym = snake.get_coriolis_rnea(root, tip)
def process(self, msg): if not self.thread_lock.acquire(False): return #msg = tf2_sensor_msgs.do_transform_cloud(msg, self.trans) #msg.header.frame_id = "base_link" #self.pub.publish(msg) pc = ros_to_pcl(msg) pc = XYZRGB_to_XYZ(pc) req = SegmentSceneRequest() req.dist_threshold = 0.07 req.angle_threshold = 0.1 req.plane_dist_threshold = 0.05 req.min_plane_size = .25 req.max_floor_height = 5. req.cluster_tolerance = 0.09 req.min_cluster_size = 0.05 req.max_cluster_size = 10 pts = np.asarray(pc).tolist() #import pdb; pdb.set_trace() for (x, y, z) in pts: if np.isnan(x): continue req.points.append(Point(x, y, z)) result = self.srv.call(req) detreq = DetectModelsRequest() detreq.models = ["soda"] detreq.max_dists_xy = [2] detreq.max_dists_z = [2] import pdb pdb.set_trace() for wall in result.walls: poly = Polygon() pointsvect = [ self.T * PyKDL.Vector(p.x, p.y, p.z) for p in wall.points ] points = np.array([[p[0], p[1], p[2]] for p in pointsvect]) #import pdb; pdb.set_trace() try: ch = CHull(points) except: continue max_x, max_y, max_z = ch.max_pts() min_x, min_y, min_z = ch.min_pts() #if min_z < 0: continue avg_z = np.mean(points, axis=0)[2] ch_xy = CHull(points[:, :2]) #surf_pts = [self.T.Inverse()*PyKDL.Vector(x, y, avg_z) for (x,y) in ch_xy.get_vertices()] #ch_pts = [Point(p[0], p[1], p[2]) for p in surf_pts] ch_pts = [Point(x, y, avg_z) for (x, y) in ch_xy.get_vertices()] poly.points = ch_pts detreq.surface_polygons.append(poly) ps = PolygonStamped(self.lastHeader, poly) ps.header.frame_id = "base_link" self.tablepub.publish(ps) tmp_pose = Pose() tmp_pose.orientation.x = 1 detreq.initial_poses = [tmp_pose] detreq.header = self.lastHeader detreq.timeout = rospy.Duration(5) detreq.initial_poses = [Pose()] detreq.header.frame_id = "base_link" try: result = self.detsrv.call(detreq) print "success!" print result except: print "failed" """ fil = pc.make_passthrough_filter() fil.set_filter_field_name("z") fil.set_filter_limits(.5, 1.5) cloud_filtered = fil.filter() seg = cloud_filtered.make_segmenter_normals(ksearch=50) seg.set_optimize_coefficients(True) seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE) seg.set_normal_distance_weight(0.05) seg.set_method_type(pcl.SAC_RANSAC) seg.set_max_iterations(300) seg.set_distance_threshold(0.03) indices, model = seg.segment() print(model) cloud_plane = cloud_filtered.extract(indices, negative=False) new_msg = self.pclToMsg(cloud_plane) self.tablepub.publish(new_msg) cloud_cyl = cloud_filtered.extract(indices, negative=True) new_msg = self.pclToMsg(cloud_cyl) self.nottablepub.publish(new_msg) seg = cloud_cyl.make_segmenter_normals(ksearch=50) seg.set_optimize_coefficients(True) seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_normal_distance_weight(0.1) seg.set_method_type(pcl.SAC_RANSAC) seg.set_max_iterations(10000) seg.set_distance_threshold(0.05) #seg.set_radius_limits(0, 0.5) indices, model = seg.segment() print(model) cloud_cylinder = cloud_cyl.extract(indices, negative=False) new_msg = self.pclToMsg(cloud_cylinder) self.pub.publish(new_msg) """ self.thread_lock.release()
def forward_velocity_kinematics(self,joint_velocities=None): end_frame = PyKDL.FrameVel() self._fk_v_kdl.JntToCart(self.joints_to_kdl('velocities',joint_velocities), end_frame) return end_frame.GetTwist()
def isPointInRange(P0,basePos,a,b) : x , y, z = intersection(P0,basePos.p, a,b) if (ifEqual(a.y(),b.y()) and a.x() <= x and b.x() >= x) or ( ifEqual(a.x(),b.x()) and a.y() <= y and b.y() >= y ) : return PyKDL.Vector(x,y,z) else : return 0
# VELMA IS READY FOR MOVING print "Preparing for grabing jar." planAndExecute(q_map_1) #Left arm is ready jarFrame = velma.getTf("B", "jar") jarX, jarY, jarZ = jarFrame.p actual_pos = velma.getLastJointState() rotZ = actual_pos[1]['torso_0_joint'] + math.pi print("podchodze do celu") Rot = PyKDL.Rotation.RotZ(rotZ) P2_global = PyKDL.Vector(jarX-0.25,jarY,jarZ+0.1) T_B_Trd = PyKDL.Frame(Rot, P2_global) velma.moveCartImpLeft([T_B_Trd], [4.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5) if velma.waitForEffectorLeft() != 0: exitError(17) rospy.sleep(0.5) #close fingers left hand and grab jar fin_angle = 0.43*math.pi dest_q = [fin_angle,fin_angle,fin_angle,0] velma.moveHandLeft(dest_q, [1,1,1,1], [2000,2000,2000,2000], 1000, hold=True) if velma.waitForHandLeft() != 0: exitError(6) rospy.sleep(0.5)
import dvrk import PyKDL as pk import time import math arm = dvrk.mtm('MTMR') # arm.home() # arm.dmove(pk.Vector(-0.001, 0.0, 0.0)) # arm.dmove(pk.Vector(0.001, 0.0, 0.0)) # print "moving forward" # # for i in range(1, 41): # arm.dmove(pk.Vector(-0.0005, 0.0, 0.0)) # # time.sleep(2) # # print "moving backward" # for i in range(1, 41): # arm.dmove(pk.Vector(0.0005, 0.0, 0.0)) # # print "movement completed" r = pk.Rotation() r.DoRotZ(math.pi / 6.0) arm.dmove(r)
def jacobian(self, joint_values=None): jacobian = PyKDL.Jacobian(self._num_jnts) self._jac_kdl.JntToJac(self.joints_to_kdl('positions', joint_values), jacobian) return self.kdl_to_mat(jacobian)
def list_to_vect(li): return kd.Vector(li[0], li[1], li[2])
def inverse_kinematics(self, position, orientation=None, seed=None, min_joints=None, max_joints=None, w_x=None, w_q=None, maxiter=500, eps=1.0e-6, with_st=False): if w_x is None and w_q is None: ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) else: ik_v_kdl = PyKDL.ChainIkSolverVel_wdls(self._arm_chain) if w_x is not None: ik_v_kdl.setWeightTS(w_x) #TS = Task Space if w_q is not None: ik_v_kdl.setWeightJS(w_q) #JS = Joint Space pos = PyKDL.Vector(position[0], position[1], position[2]) if orientation is not None: rot = PyKDL.Rotation() rot = rot.Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) # Populate seed with current angles if not provided seed_array = PyKDL.JntArray(self._num_jnts) if seed is not None: seed_array.resize(len(seed)) for idx, jnt in enumerate(seed): seed_array[idx] = jnt else: seed_array = self.joints_to_kdl('positions') # Make IK Call if orientation is not None: goal_pose = PyKDL.Frame(rot, pos) else: goal_pose = PyKDL.Frame(pos) result_angles = PyKDL.JntArray(self._num_jnts) # Make IK solver with joint limits if min_joints is None: min_joints = self.joint_limits_lower if max_joints is None: max_joints = self.joint_limits_upper mins_kdl = PyKDL.JntArray(len(min_joints)) for idx, jnt in enumerate(min_joints): mins_kdl[idx] = jnt maxs_kdl = PyKDL.JntArray(len(max_joints)) for idx, jnt in enumerate(max_joints): maxs_kdl[idx] = jnt ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, ik_v_kdl, maxiter, eps) if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) if with_st: return True, result else: return result else: if with_st: result = np.array(list(result_angles)) return False, result else: return None
class PickAndPlaceState(Enum): OPEN_JAW = 1, APPROACH_OBJECT = 2, GRAB_OBJECT = 3, CLOSE_JAW = 4, APPROACH_DEST = 5, DROP_OBJECT = 6, HOME = 7, DONE = 8 VECTOR_EPS = 0.005 DOWN_JAW_ORIENTATION = PyKDL.Rotation.RPY(math.pi, 0, -math.pi / 2.0) PSM_HOME_POS = PyKDL.Vector(0., 0., -0.1) def vector_eps_eq(lhs, rhs): return bool((lhs - rhs).Norm() < 0.005) class PickAndPlaceStateMachine: def jaw_fully_open(self): return True if self.psm.get_current_jaw_position( ) >= math.pi / 3 else False def jaw_fully_closed(self): return True if self.psm.get_current_jaw_position() <= 0 else False def _open_jaw(self):
def point2kdl(point): return kdl.Vector(point.x, point.y, point.z)
type=float, help="y-coordinate (in map) of the imaginary object") parser.add_argument("z", type=float, help="z-coordinate (in map) of the imaginary object") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("test_grasping") robot = get_robot(args.robot) entity_id = "test_item" pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)), frame_id="/map") robot.ed.update_entity(id=entity_id, frame_stamped=pose) shape = RightPrism([ kdl.Vector(0, 0, 0), kdl.Vector(0, 0.05, 0), kdl.Vector(0.05, 0.05, 0), kdl.Vector(0.05, 0, 0) ], -0.1, 0.1) item = Entity(entity_id, "test_type", pose.frame_id, pose.frame, shape, None, None, None) item = ds.Designator(item) arm = ds.UnoccupiedArmDesignator(robot, arm_properties={
def _set_arm_dest(self, dest): if self.log_verbose: loginfo("Setting {} dest to {}".format(self.psm.name(), dest)) if self.psm.get_desired_position().p != dest: self.psm.move(PyKDL.Frame(DOWN_JAW_ORIENTATION, dest), blocking=False)
def transform_to_kdl(self,t): return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z))
def __init__(self, limb, ee_frame_name="panda_link8", additional_segment_config=None, description=None): if description is None: self._franka = URDF.from_parameter_server(key='robot_description') else: self._franka = URDF.from_xml_file(description) self._kdl_tree = kdl_tree_from_urdf_model(self._franka) if additional_segment_config is not None: for c in additional_segment_config: q = quaternion.from_rotation_matrix(c["origin_ori"]).tolist() kdl_origin_frame = PyKDL.Frame( PyKDL.Rotation.Quaternion(q.x, q.y, q.z, q.w), PyKDL.Vector(*(c["origin_pos"].tolist()))) kdl_sgm = PyKDL.Segment(c["child_name"], PyKDL.Joint(c["joint_name"]), kdl_origin_frame, PyKDL.RigidBodyInertia()) self._kdl_tree.addSegment(kdl_sgm, c["parent_name"]) self._base_link = self._franka.get_root() self._tip_link = ee_frame_name self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._limb_interface = limb self._joint_names = deepcopy(self._limb_interface.joint_names()) self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def get_jacobian(self, joint): jac = PyKDL.Jacobian(self.kine_chain.getNrOfJoints()) self.jac_calc.JntToJac(joint, jac) return jac
# The ONLY objects the collision detection software is aware # of are itself & the floor. if __name__ == '__main__': rospy.init_node('ar_detect_and_grab') cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) listener = tf.TransformListener() try: listener.waitForTransform('/base_link', '/card_reader', rospy.Time(0), rospy.Duration(4.0)) (trans, rot) = listener.lookupTransform('/base_link', '/card_reader', rospy.Time(0)) res = Pose(Point(*trans), Quaternion(*rot)) frame_gripper_link = PyKDL.Frame(PyKDL.Vector(-0.166, 0.0, 0.0)) (trans_res, rot_res) = tf_conversions.posemath.toTf( tf_conversions.posemath.fromMsg(res) * frame_gripper_link) frame_prep_link = PyKDL.Frame(PyKDL.Vector(-0.166, 0.0, 0.1)) (trans_prep, rot_prep) = tf_conversions.posemath.toTf( tf_conversions.posemath.fromMsg(res) * frame_prep_link) except (tf.LookupException, tf.ConnectivityException): print 'couldnt get transform to handle' # Create move group interface for a fetch robot move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link")
def get_objects_and_img(left_image_msg, right_image_msg, stereo_cam_model, cam_to_world_tf): # this gets the position of the red ball thing in the camera frame # and the image with X's on the desired features fp = feature_processor(FEAT_PATHS) left_feats, left_frame = fp.FindImageFeatures(left_image_msg) right_feats, right_frame = fp.FindImageFeatures(right_image_msg) # discard features with image y > bowl y left_bowl = None for left_feat in left_feats: if left_feat.type == FeatureType.BOWL: left_bowl = left_feat left_feats = filter(lambda feat: feat.pos[1] >= left_bowl.pos[1], left_feats) left_feats.sort(key=lambda feat: feat.pos[1], reverse=False) right_bowl = None for right_feat in right_feats: if right_feat.type == FeatureType.BOWL: right_bowl = right_feat right_feats = filter(lambda feat: feat.pos[1] >= right_bowl.pos[1], right_feats) right_feats.sort(key=lambda feat: feat.pos[1], reverse=False) matched_feats = [] for left_feat in left_feats: same_color_feats = filter( lambda right_feat: right_feat.color == left_feat.color, right_feats) if not same_color_feats: rospy.logwarn( "Failed to match left detection {} to a right detection!". format(left_feat)) continue matched_feats.append((left_feat, min(same_color_feats, key=lambda right_feat: (right_feat.pos[0] - left_feat.pos[0]) ** 2 \ + (right_feat.pos[1] - left_feat.pos[1]) ** 2))) objects = [] for left_feat, right_feat in matched_feats: disparity = abs(left_feat.pos[0] - right_feat.pos[0]) pos_cv = stereo_cam_model.projectPixelTo3d(left_feat.pos, float(disparity)) # there's a fixed rotation to convert this to the camera coordinate frame pos_cam = np.matmul(CV_TO_CAM_FRAME_ROT, pos_cv) pos = None if cam_to_world_tf is not None: pos = cam_to_world_tf * PyKDL.Vector(*pos_cam) else: pos = PyKDL.Vector(*pos_cam) if left_feat.color != right_feat.color: rospy.loginfo("Color mismatch between left and right detection") objects.append(Object3d(pos, left_feat.type, left_feat.color)) return objects, np.hstack((left_frame, right_frame))
def linkStatesCallback(self, msg): self.getNameOrder(msg.name) q_world = PyKDL.Rotation.Quaternion(msg.pose[0].orientation.x, msg.pose[0].orientation.y, msg.pose[0].orientation.z, msg.pose[0].orientation.w) q_base = PyKDL.Rotation.Quaternion(msg.pose[self.order[0]].orientation.x, msg.pose[self.order[0]].orientation.y, msg.pose[self.order[0]].orientation.z, msg.pose[self.order[0]].orientation.w) if self.Gtype == 'reflex' or self.Gtype == 'model_O': q_1_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[1]].orientation.x, msg.pose[self.order[1]].orientation.y, msg.pose[self.order[1]].orientation.z, msg.pose[self.order[1]].orientation.w) q_1_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[2]].orientation.x, msg.pose[self.order[2]].orientation.y, msg.pose[self.order[2]].orientation.z, msg.pose[self.order[2]].orientation.w) q_1_3 = PyKDL.Rotation.Quaternion(msg.pose[self.order[3]].orientation.x, msg.pose[self.order[3]].orientation.y, msg.pose[self.order[3]].orientation.z, msg.pose[self.order[3]].orientation.w) q_2_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[4]].orientation.x, msg.pose[self.order[4]].orientation.y, msg.pose[self.order[4]].orientation.z, msg.pose[self.order[4]].orientation.w) q_2_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[5]].orientation.x, msg.pose[self.order[5]].orientation.y, msg.pose[self.order[5]].orientation.z, msg.pose[self.order[5]].orientation.w) q_2_3 = PyKDL.Rotation.Quaternion(msg.pose[self.order[6]].orientation.x, msg.pose[self.order[6]].orientation.y, msg.pose[self.order[6]].orientation.z, msg.pose[self.order[6]].orientation.w) q_3_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[7]].orientation.x, msg.pose[self.order[7]].orientation.y, msg.pose[self.order[7]].orientation.z, msg.pose[self.order[7]].orientation.w) q_3_3 = PyKDL.Rotation.Quaternion(msg.pose[self.order[8]].orientation.x, msg.pose[self.order[8]].orientation.y, msg.pose[self.order[8]].orientation.z, msg.pose[self.order[8]].orientation.w) # Velocity should be added here else: q_1_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[1]].orientation.x, msg.pose[self.order[1]].orientation.y, msg.pose[self.order[1]].orientation.z, msg.pose[self.order[1]].orientation.w) q_1_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[2]].orientation.x, msg.pose[self.order[2]].orientation.y, msg.pose[self.order[2]].orientation.z, msg.pose[self.order[2]].orientation.w) q_2_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[4]].orientation.x, msg.pose[self.order[4]].orientation.y, msg.pose[self.order[4]].orientation.z, msg.pose[self.order[4]].orientation.w) q_2_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[5]].orientation.x, msg.pose[self.order[5]].orientation.y, msg.pose[self.order[5]].orientation.z, msg.pose[self.order[5]].orientation.w) dq_1_1 = msg.twist[self.order[1]].angular.z dq_1_2 = msg.twist[self.order[2]].angular.z dq_2_1 = msg.twist[self.order[4]].angular.z dq_2_2 = msg.twist[self.order[5]].angular.z if self.Gtype == 'reflex': # self.joint_angles = [f1_1,f1_2,f1_3,f2_1,f2_2,f2_3,f3_2,f3_3] self.joint_angles[0] = (q_1_1.Inverse()*q_base).GetEulerZYX()[0] a = q_1_1.UnitZ()*q_1_2.UnitX() if a[1] > 0: self.joint_angles[1] = np.pi/2-np.arccos(PyKDL.dot(q_1_1.UnitZ(), q_1_2.UnitX()))+0.2807829 else: self.joint_angles[1] = np.pi/2+np.arccos(PyKDL.dot(q_1_1.UnitZ(), q_1_2.UnitX()))+0.2807829 self.joint_angles[2] = (q_1_3.Inverse()*q_1_2).GetEulerZYX()[1] self.joint_angles[3] = (q_2_1.Inverse()*q_base).GetEulerZYX()[0] a = q_2_1.UnitZ()*q_2_2.UnitX() if a[1] > 0: self.joint_angles[4] = np.pi/2-np.arccos(PyKDL.dot(q_2_1.UnitZ(), q_2_2.UnitX()))+0.2807829 else: self.joint_angles[4] = np.pi/2+np.arccos(PyKDL.dot(q_2_1.UnitZ(), q_2_2.UnitX()))+0.2807829 self.joint_angles[5] = (q_2_3.Inverse()*q_2_2).GetEulerZYX()[1] a = q_base.UnitZ()*q_3_2.UnitX() if a[1] < 0: self.joint_angles[6] = np.pi/2-np.arccos(PyKDL.dot(q_base.UnitZ(), q_3_2.UnitX()))+0.2807829 else: self.joint_angles[6] = np.pi/2+np.arccos(PyKDL.dot(q_base.UnitZ(), q_3_2.UnitX()))+0.2807829 self.joint_angles[7] = (q_3_3.Inverse()*q_3_2).GetEulerZYX()[1] if self.Gtype == 'model_O': # self.joint_angles = [f1_1,f1_2,f1_3,f2_1,f2_2,f2_3,f3_2,f3_3] self.joint_angles[0] = np.mod(np.pi-(q_1_1.Inverse()*q_base).GetEulerZYX()[2], np.pi) self.joint_angles[1] = -(q_1_2.Inverse()*q_1_1).GetEulerZYX()[0] self.joint_angles[2] = -(q_1_3.Inverse()*q_1_2).GetEulerZYX()[0] self.joint_angles[3] = np.pi+(q_2_1.Inverse()*q_base).GetEulerZYX()[2] # Changes sign based on the rotation of the base - not currently important self.joint_angles[4] = -(q_2_2.Inverse()*q_2_1).GetEulerZYX()[0] self.joint_angles[5] = -(q_2_3.Inverse()*q_2_2).GetEulerZYX()[0] self.joint_angles[6] = -(q_3_2.Inverse()*q_base).GetEulerZYX()[0] self.joint_angles[7] = -(q_3_3.Inverse()*q_3_2).GetEulerZYX()[0] if self.Gtype == 'model_T42': a = (q_1_1.Inverse()*q_base).GetEulerZYX() if a[0] < 0: self.joint_angles[0] = -a[1] else: self.joint_angles[0] = np.pi+a[1] self.joint_angles[1] = -(q_1_2.Inverse()*q_1_1).GetEulerZYX()[2] a = (q_2_1.Inverse()*q_base).GetEulerZYX() if a[0] < 0: self.joint_angles[2] = -a[1] else: self.joint_angles[2] = np.pi+a[1] self.joint_angles[3] = -(q_2_2.Inverse()*q_2_1).GetEulerZYX()[2] # Apply mean filter to joint velocities with windowSize if self.win.shape[0] < windowSize: self.joint_vels = np.array([-dq_1_1, -(dq_1_2 - dq_1_1), dq_2_1, dq_2_2 - dq_2_1]) # Currently finger velocity do not compensate base motion self.win = np.append(self.win, self.joint_vels).reshape(-1, 4) else: v = np.array([-dq_1_1, -(dq_1_2 - dq_1_1), dq_2_1, dq_2_2 - dq_2_1]) # Currently finger velocity do not compensate base motion self.win = np.append(self.win, v).reshape(-1, 4) self.joint_vels = np.mean(self.win, axis=0) self.win = np.delete(self.win, 0, axis=0) self.joint_vels[np.abs(self.joint_vels) <= 0.1e-2] = 0.
go = True if key == ord("a"): # PRESS 'a' to RESET arm_right.reset(top_table_pose) if point is None: print("No Point") continue ###################### CONTROL ######################## imgr_center = np.array(imgr.shape[0:2]) / 2 imgr_center = np.flip(imgr_center) ctrl_x, ctrl_y = -kp_xy * (point - imgr_center) print("%8.4f, %8.4f" % (ctrl_x, ctrl_y)) # translation vector (wrt image rf) tr_ctrl = PyKDL.Vector(ctrl_x, ctrl_y, 0) # rotation from image rf to camera rf R_img2cam = PyKDL.Rotation().RotZ(math.pi) frame_cam2eef = arm_right.gettf(frame_id="right_hand_camera", parent_id="right_gripper") # rotation from camera rf to gripper rf R_cam2eef = frame_cam2eef.M # translation vector (wrt gripper rf) tr_ctrl = R_cam2eef * R_img2cam * tr_ctrl # target frame (wrt gripper rf) frame_ctrl = PyKDL.Frame() frame_ctrl.p = tr_ctrl
def dict_to_frame(di): return kd.Frame(kd.Rotation.RPY(di['rx'], di['ry'], di['rz']), kd.Vector(di['tx'], di['ty'], di['tz']))
#u2c gantry = u2c.URDFparser() gantry.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) pbmodel = pb.loadURDF(path_to_urdf, useFixedBase=True, flags=pb.URDF_USE_INERTIA_FROM_FILE) pb.setGravity(0, 0, -9.81) #joint info jointlist, names, q_max, q_min = gantry.get_joint_info(root, tip) n_joints = gantry.get_n_joints(root, tip) q_kdl = kdl.JntArray(n_joints) #declarations q_kdl = kdl.JntArray(n_joints) #kdl q_kdl = kdl.JntArray(n_joints) gravity_kdl = kdl.Vector() gravity_kdl[2] = -9.81 g_kdl = kdl.JntArray(n_joints) #u2c & pybullet q = [None] * n_joints qdot = [None] * n_joints zeros_pb = [None] * n_joints gravity_u2c = [0, 0, -9.81] g_sym = gantry.get_gravity_rnea(root, tip, gravity_u2c)
def dict_to_vect(di): return kd.Vector(di['tx'], di['ty'], di['tz'])
def getMovementTime(self, T_B_Wd, max_v_l = 0.1, max_v_r = 0.2): self.updateTransformations() twist = PyKDL.diff(self.T_B_W, T_B_Wd, 1.0) v_l = twist.vel.Norm() v_r = twist.rot.Norm() # print "v_l: %s v_r: %s"%(v_l, v_r) f_v_l = v_l/max_v_l f_v_r = v_r/max_v_r if f_v_l > f_v_r: duration = f_v_l else: duration = f_v_r if duration < 0.2: duration = 0.2 return duration