def space_from_joints(joint_angles): T01, T12, T23, T34, T4e = direct_kinematics(joint_angles) T = tfs.concatenate_matrices(T01, T12, T23, T34, T4e) rz1, ry, rz2 = tfs.euler_from_matrix(T, 'szyz') trans = tfs.translation_from_matrix(T) S = np.append(trans, rz2) return S
def pack_pose(time, child, parent, matrix=None, trans=None, quat=None): if matrix is not None and (trans is None and quat is None): trans = tfs.translation_from_matrix(matrix) quat = tfs.quaternion_from_matrix(matrix) elif matrix is None and trans is not None and quat is not None: matrix = None # nothing else: print 'invalid use' t = TransformStamped() t.header.frame_id = parent t.header.stamp = time t.child_frame_id = child t.transform.translation.x = trans[0] t.transform.translation.y = trans[1] t.transform.translation.z = trans[2] quat = numpy.array(quat) quat = quat / numpy.linalg.norm(quat, ord=2) t.transform.rotation.x = quat[0] t.transform.rotation.y = quat[1] t.transform.rotation.z = quat[2] t.transform.rotation.w = quat[3] return t
def wrench_callback(self, state): #calculating force estimate from position error (does not compensate for motion error due to dynamics) q = self.robot.get_joint_angles() pos,_ = self.robot.kinematics.FK(q) x_err = np.matrix([state.point.x-pos[0,0], state.point.y-pos[1,0], state.point.z-pos[2,0]]).T ##########This was for debugging purposes # ps = PointStamped() # ps.header.frame_id = '/torso_lift_link' # ps.header.stamp = rospy.get_rostime() # ps.point.x = x_dot_err[0,0] # ps.point.y = x_dot_err[0,0] # ps.point.z = 0 #self.err_pub.publish(ps) feedback = -1.0*self.kp*x_err # - self.kd*x_dot_err This term is now included in real-time omni driver #find and use the rotation matrix from wrench to torso df_R_ee = tfu.rotate(self.dest_frame, 'torso_lift_link', self.tflistener) wr_df = self.force_scaling*np.array(tr.translation_from_matrix(df_R_ee * tfu.translation_matrix([feedback[0,0], feedback[1,0], feedback[2,0]]))) #limiting the max and min force feedback sent to omni wr_df = np.where(wr_df>self.omni_max_limit, self.omni_max_limit, wr_df) wr_df = np.where(wr_df<self.omni_min_limit, self.omni_min_limit, wr_df) wr = OmniFeedback() wr.force.x = wr_df[0] wr.force.y = wr_df[1] wr.force.z = wr_df[2] if self.enable == True: self.omni_fb.publish(wr)
def _makePreGraspPose(self, boxMat, axis): if axis==0: #x axis alpha = 0 gamma = 0 else: #y axis alpha = pi/2 gamma = -pi/2 ik_request = PositionIKRequest() ik_request.ik_link_name = self.toolLinkName with self._joint_state_lock: ik_request.ik_seed_state.joint_state = copy.deepcopy(self._joint_states) ik_request.pose_stamped = PoseStamped() ik_request.pose_stamped.header.stamp = rospy.Time.now() ik_request.pose_stamped.header.frame_id = self.frameID beta = pi/2 while beta < 3*pi/2: rotationMatrix = transformations.euler_matrix(alpha, beta, gamma, 'rzyz') distance = self.preGraspDistance + self.gripperFingerLength preGraspMat = transformations.translation_matrix([0,0,-distance]) fullMat = transformations.concatenate_matrices(boxMat, rotationMatrix, preGraspMat) p = transformations.translation_from_matrix(fullMat) q = transformations.quaternion_from_matrix(fullMat) print "trying {} radians".format(beta) try: self._ik_server(ik_request, Constraints(), rospy.Duration(5.0)) except rospy.service.ServiceException: beta += 0.1 else: if ik_resp.error_code.val > 0: return beta else: #print ik_resp.error_code.val beta += 0.01 rospy.logerr('No way to pick this up. All IK solutions failed.') return 7 * pi / 6
def space_from_joints_small(joint_angles): T01, T12, T23, T34, T45, T5e, Tec, Tem = direct_kinematics(joint_angles) T = tfs.concatenate_matrices(T01, T12, T23, T34, T45, T5e, Tem) rx, ry, rz = tfs.euler_from_matrix(T, 'sxyz') trans = tfs.translation_from_matrix(T) S = np.concatenate((trans, [rz, ry]), axis=1) return S
def transformPose(self, target_frame, ps): """ :param target_frame: the tf target frame, a string :param ps: the geometry_msgs.msg.PoseStamped message :return: new geometry_msgs.msg.PoseStamped message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message. """ # mat44 is frame-to-frame transform as a 4x4 mat44 = self.asMatrix(target_frame, ps.header) # pose44 is the given pose as a 4x4 pose44 = numpy.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation)) # txpose is the new pose in target_frame as a 4x4 txpose = numpy.dot(mat44, pose44) # xyz and quat are txpose's position and orientation xyz = tuple(transformations.translation_from_matrix(txpose))[:3] quat = tuple(transformations.quaternion_from_matrix(txpose)) # assemble return value PoseStamped r = geometry_msgs.msg.PoseStamped() r.header.stamp = ps.header.stamp r.header.frame_id = target_frame r.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat)) return r
def computeTransformation(world, slam, camera): # Here we do not care about the slamMworld transformation # timing as it is constant. tWorld = listener.getLatestCommonTime(world, camera) tMap = listener.getLatestCommonTime(slam, camera) t = min(tWorld, tMap) # Current pose given by the SLAM. (slamMcam_T, slamMcam_Q) = listener.lookupTransform(camera, slam, t) slamMcam = np.matrix(transformer.fromTranslationRotation(slamMcam_T, slamMcam_Q)) # Estimation of the camera position given by control. #FIXME: order is weird but it works. (worldMcam_T, worldMcam_Q) = listener.lookupTransform(world, camera, t) worldMcam = np.matrix(transformer.fromTranslationRotation(worldMcam_T, worldMcam_Q)) (slamMworld_T, slamMworld_Q) = listener.lookupTransform(slam, world, t) slamMworld = np.matrix(transformer.fromTranslationRotation(slamMworld_T, slamMworld_Q)) slamMcamEstimated = slamMworld * worldMcam # Inverse frames. camMslam = np.linalg.inv(slamMcam) camMslamEstimated = np.linalg.inv(slamMcamEstimated) # Split. camMslam_T = translation_from_matrix(camMslam) camMslam_Q = quaternion_from_matrix(camMslam) camMslam_E = euler_from_matrix(camMslam) camMslamEstimated_T = translation_from_matrix(camMslamEstimated) camMslamEstimated_Q = quaternion_from_matrix(camMslamEstimated) camMslamEstimated_E = euler_from_matrix(camMslamEstimated) # Compute correction. camMslamCorrected_T = [camMslam_T[0], camMslamEstimated_T[1], camMslam_T[2]] camMslamCorrected_E = [camMslamEstimated_E[0], camMslam_E[1], camMslamEstimated_E[2]] camMslamCorrected_Q = quaternion_from_euler(*camMslamCorrected_E) return (camMslamCorrected_T, camMslamCorrected_Q, t)
def getUnitTranslationVector(self): d = self.getPositionDistance() if(d == 0): return (0,0,0) else: t = translation_from_matrix(self._matrix) return (t[0]/d, t[1]/d, t[2]/d)
def _makeGraspPath(self, preGraspGoal): '''Given a pregrasp MoveArmGoal message, generate a MoveArmGoal message to perform the final approach to the object to grasp it.''' graspGoal = MoveArmGoal() graspGoal.planner_service_name = self.plannerServiceName motion_plan_request = MotionPlanRequest() motion_plan_request.group_name = self.armGroupName motion_plan_request.num_planning_attempts = 25 motion_plan_request.planner_id = "" motion_plan_request.allowed_planning_time = rospy.Duration(5,0) #Orientation constraint is the same as for pregrasp motion_plan_request.goal_constraints.orientation_constraints = copy.deepcopy(preGraspGoal.motion_plan_request.goal_constraints.orientation_constraints) #motion_plan_request.goal_constraints.orientation_constraints[0].orientation = Quaternion(0.656778, 0.261999, 0.648401, -0.282093) #stow orientation for debug graspGoal.motion_plan_request = motion_plan_request #Translate from pregrasp position to final position in a roughly straight line o = motion_plan_request.goal_constraints.orientation_constraints[0].orientation p = preGraspGoal.motion_plan_request.goal_constraints.position_constraints[0].position preGraspMat = transformations.quaternion_matrix([o.x,o.y,o.z,o.w]) preGraspMat[:3, 3] = [p.x,p.y,p.z] distance = self.preGraspDistance * .5 + self.gripperFingerLength * .5 graspTransMat = transformations.translation_matrix([0,0,distance]) graspMat = transformations.concatenate_matrices(preGraspMat, graspTransMat) #print preGraspMat #print graspTransMat #print graspMat p = transformations.translation_from_matrix(graspMat) #Publish grasp transform for visualization self._tf_broadcaster.sendTransform( (p[0],p[1],p[2]), (o.x, o.y, o.x, o.w), motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp, "grasp", motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id) pos_constraint = PositionConstraint() pos_constraint.header = motion_plan_request.goal_constraints.orientation_constraints[0].header pos_constraint.link_name = self.toolLinkName pos_constraint.position = Point(p[0],p[1],p[2]) #pos_constraint.position = Point(-0.0644721, 0.609922, 0) #Stow position for debug pos_constraint.constraint_region_shape.type = Shape.SPHERE pos_constraint.constraint_region_shape.dimensions = [0.01] pos_constraint.weight = 1 motion_plan_request.goal_constraints.position_constraints.append(pos_constraint) #TODO: Add path constraint to require a (roughly) cartesian move #Turn off collision operations between the gripper and all objects for collisionName in self.gripperCollisionNames: collisionOperation = CollisionOperation(collisionName, CollisionOperation.COLLISION_SET_ALL, 0.0, CollisionOperation.DISABLE) graspGoal.operations.collision_operations.append(collisionOperation) return graspGoal
def get_relative_pose(object_a, object_b): object_a_mat = get_object_pose_as_matrix(object_a) object_b_mat = get_object_pose_as_matrix(object_b) rel_mat = numpy.linalg.inv(object_a_mat).dot(object_b_mat) trans = transformations.translation_from_matrix(rel_mat) rot = transformations.quaternion_from_matrix(rel_mat) rot = [rot[3], rot[0], rot[1], rot[2]] print "pose: [%f, %f, %f, %f, %f, %f, %f]" % (trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3]) return (trans, rot)
def _makeMove(self): '''Move forward a small distance''' print "Making move goal" graspGoal = MoveArmGoal() graspGoal.planner_service_name = self.plannerServiceName motion_plan_request = MotionPlanRequest() motion_plan_request.group_name = self.armGroupName motion_plan_request.num_planning_attempts = 5 motion_plan_request.planner_id = "" motion_plan_request.allowed_planning_time = rospy.Duration(5,0) #Orientation constraint is the same as for previous #Create Orientation constraint object o_constraint = OrientationConstraint() o_constraint.header.frame_id = self.frameID o_constraint.header.stamp = rospy.Time.now() o_constraint.link_name = self.toolLinkName o_constraint.orientation = Quaternion(0.656788, 0.261971, 0.648416, -0.282059) o_constraint.absolute_roll_tolerance = 0.04 o_constraint.absolute_pitch_tolerance = 0.04 o_constraint.absolute_yaw_tolerance = 0.04 o_constraint.weight = 1 motion_plan_request.goal_constraints.orientation_constraints.append(o_constraint) #Translate from pregrasp position to final position in a roughly straight line o = o_constraint.orientation p = Point(-0.064433, 0.609915, 0) preGraspMat = transformations.quaternion_matrix([o.x,o.y,o.z,o.w]) preGraspMat[:3, 3] = [p.x,p.y,p.z] distance = .3#self.preGraspDistance + self.gripperFingerLength/2 graspTransMat = transformations.translation_matrix([0,0,distance]) graspMat = transformations.concatenate_matrices(preGraspMat, graspTransMat) #print preGraspMat #print graspTransMat #print graspMat p = transformations.translation_from_matrix(graspMat) #Publish grasp transform for visualization self._tf_broadcaster.sendTransform( (p[0],p[1],p[2]), (o.x, o.y, o.x, o.w), o_constraint.header.stamp, "grasp", o_constraint.header.frame_id) pos_constraint = PositionConstraint() pos_constraint.header = o_constraint.header pos_constraint.link_name = self.toolLinkName pos_constraint.position = Point(p[0],p[1],p[2]) pos_constraint.constraint_region_shape.type = Shape.SPHERE pos_constraint.constraint_region_shape.dimensions = [0.01]#[0.01, 0.01, 0.01] pos_constraint.weight = 1 motion_plan_request.goal_constraints.position_constraints.append(pos_constraint) graspGoal.motion_plan_request = motion_plan_request return graspGoal
def run(self): r = rospy.Rate(50) while not rospy.is_shutdown(): try: common_link = "/base_link" c_T_rgrip = tfu.transform(common_link, "/r_gripper_tool_frame", self.tflistener) c_T_lgrip = tfu.transform(common_link, "/l_gripper_tool_frame", self.tflistener) gripper_right_c = np.matrix(tr.translation_from_matrix(c_T_rgrip * tr.translation_matrix([0, 0, 0.0]))) gripper_left_c = np.matrix(tr.translation_from_matrix(c_T_lgrip * tr.translation_matrix([0, 0, 0.0]))) look_at_point_c = ((gripper_right_c + gripper_left_c) / 2.0).A1.tolist() g = PointHeadGoal() g.target.header.frame_id = "/base_link" g.target.point = Point(*look_at_point_c) g.min_duration = rospy.Duration(1.0) g.max_velocity = 10.0 self.head_client.send_goal(g) self.head_client.wait_for_result(rospy.Duration(1.0)) r.sleep() except tf.LookupException, e: print e
def mat2pose(m): trans = tr.translation_from_matrix(m) rot = tr.quaternion_from_matrix(m) p = Pose() p.position.x = trans[0] p.position.y = trans[1] p.position.z = trans[2] p.orientation.x = rot[0] p.orientation.y = rot[1] p.orientation.z = rot[2] p.orientation.w = rot[3] return p
def transformPose(mat44, ps): # pose44 is the given pose as a 4x4 pose44 = numpy.dot(tf.listener.xyz_to_mat44(ps.pose.position), tf.listener.xyzw_to_mat44(ps.pose.orientation)) # txpose is the new pose in target_frame as a 4x4 txpose = numpy.dot(mat44, pose44) # xyz and quat are txpose's position and orientation xyz = tuple(transformations.translation_from_matrix(txpose))[:3] quat = tuple(transformations.quaternion_from_matrix(txpose)) # assemble return value Pose return geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat))
def get_current_pose(self): frame_described_in = str(self.frame_box.currentText()) arm = tu.selected_radio_button(self.arm_radio_buttons).lower() if arm == 'right': arm_tip_frame = VelocityPriorityMoveTool.RIGHT_TIP else: arm_tip_frame = VelocityPriorityMoveTool.LEFT_TIP self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Time(), rospy.Duration(2.)) p_arm = tfu.tf_as_matrix(self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0))) trans, rotation = tr.translation_from_matrix(p_arm), tr.quaternion_from_matrix(p_arm) for value, vr in zip(trans, [self.xline, self.yline, self.zline]): vr.setValue(value) for value, vr in zip(tr.euler_from_quaternion(rotation), [self.phi_line, self.theta_line, self.psi_line]): vr.setValue(np.degrees(value))
def calculate_bridge_and_base_pose(self, table_state, phi): # Calculate bridge pose from cue position and phi phi_radians = math.pi * phi / 180.0 cue_x, cue_y = self.get_cue_ball_pos(table_state) bridge_x, bridge_y = self.get_bridge_pos(cue_x, cue_y, phi) (qx, qy, qz, qw) = transformations.quaternion_about_axis(phi_radians, (0, 0, 1)) bridge_pos = transformations.translation_matrix([bridge_x, bridge_y, 0.0]) bridge_orient = transformations.quaternion_matrix([qx, qy, qz, qw]) bridge_pose = transformations.concatenate_matrices(bridge_pos, bridge_orient) bridge_pose_pos = transformations.translation_from_matrix(bridge_pose) bridge_pose_orient = transformations.quaternion_from_matrix(bridge_pose) # Produce base pose via fixed transform from bridge pose bridge_to_base_trans = transformations.translation_matrix([Constants.BRIDGE_IN_BASE_X, Constants.BRIDGE_IN_BASE_Y, Constants.BRIDGE_IN_BASE_Z]) bridge_to_base_rot = transformations.quaternion_matrix([Constants.BRIDGE_IN_BASE_QX, Constants.BRIDGE_IN_BASE_QY, Constants.BRIDGE_IN_BASE_QZ, Constants.BRIDGE_IN_BASE_QW]) bridge_to_base = transformations.concatenate_matrices(bridge_to_base_trans, bridge_to_base_rot) base_to_bridge = transformations.inverse_matrix(bridge_to_base) base_pose = transformations.concatenate_matrices(bridge_pose, base_to_bridge) # correct order? base_pose_pos = transformations.translation_from_matrix(base_pose) base_pose_orient = transformations.quaternion_from_matrix(base_pose) print "BASE_POSE", base_pose, "BRIDGE_POSE", bridge_pose return ((bridge_pose_pos, bridge_pose_orient), (base_pose_pos, base_pose_orient))
def calENU(self): if(self.body.isNone() or self.headingAngle is None): return False else: br = self.body.getRotationAroundZ() q = rotation_matrix(br+self.headingAngle, (0,0,1)) t = translation_from_matrix(self.body.getMatrix()) t = translation_matrix(t) self.enu.setMatrix(t).transformByMatrix(q) return True #eoc
def move(self, newPlan): br = tf.TransformBroadcaster() rate = rospy.Rate(1.0/self.dt) for joints in newPlan: for k in range(0, len(joints)): self.pubs[k].publish(joints[k]) self.joints = joints rate.sleep() T = self.matrix_from_joint(joints) q = tfs.quaternion_from_matrix(T) t = tfs.translation_from_matrix(T) br.sendTransform(t, q, rospy.Time.now(), "new pose", "base")
def publish_result(gmodel): result = gmodel.grasps print "the total grasp is %d" % len(result) i = 0 pose_array = geometry_msgs.msg.PoseArray() for grasp in result: Tgrasp = gmodel.getGlobalGraspTransform(grasp) xyz = tuple(transformations.translation_from_matrix(Tgrasp))[:3] quat = tuple(transformations.quaternion_from_matrix(Tgrasp)) # assemble return value PoseStampe pose_array.poses.append(geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat))) pose_array.header.frame_id = "/kinfu_origin" pose_array.header.stamp = rospy.Time.now() grasp_array_pub.publish(pose_array) rospy.on_shutdown(shut_down_hook)
def tfm(matrix, parent, child, stamp): rotation = quaternion_from_matrix(matrix) translation = translation_from_matrix(matrix) t = TransformStamped() t.header.frame_id = parent t.header.stamp = stamp t.child_frame_id = child t.transform.translation.x = translation[0] t.transform.translation.y = translation[1] t.transform.translation.z = translation[2] t.transform.rotation.x = rotation[0] t.transform.rotation.y = rotation[1] t.transform.rotation.z = rotation[2] t.transform.rotation.w = rotation[3] return tfMessage([t])
def __init__(self, robot, center_in_torso_frame, #=[1,.3,-1], scaling_in_base_frame, #=[3.5, 3., 5.], omni_name, #='omni1', set_goal_pose_topic, # = 'l_cart/command_pose', tfbroadcast=None, #=None, tflistener=None): #=None): #threading.Thread.__init__(self) self.enabled = False self.zero_out_forces = True self.robot = robot #self.X_BOUNDS = [.3, 1.5] #Bound translation of PR2 arms in the X direction (in torso link frame) self.kPos = 15. self.kVel = 0.5 self.kPos_close = 70. self.omni_name = omni_name self.center_in_torso_frame = center_in_torso_frame self.scaling_in_base_frame = scaling_in_base_frame self.tflistener = tflistener self.tfbroadcast = tfbroadcast self.prev_dt = 0.0 q = self.robot.get_joint_angles() self.tip_tt, self.tip_tq = self.robot.kinematics.FK(q, self.robot.kinematics.n_jts) # self.tip_tt = np.zeros((3,1)) # self.tip_tq = np.zeros((4,1)) self.teleop_marker_pub = rospy.Publisher('/teleop/viz/goal', Marker) rate = rospy.Rate(100.0) self.omni_fb = rospy.Publisher(self.omni_name + '_force_feedback', OmniFeedback) self.set_goal_pub = rospy.Publisher(set_goal_pose_topic, PoseStamped) rospy.loginfo('Attempting to calculate scaling from omni to arm workspace') success = False while not success and (not rospy.is_shutdown()): rate.sleep() try: m = tfu.translation_matrix(self.scaling_in_base_frame) vec_l0 = tr.translation_from_matrix(tfu.rotate(self.omni_name + '_center', '/torso_lift_link', self.tflistener)*m) self.scale_omni_l0 = np.abs(vec_l0) success = True except tf.LookupException, e: pass except tf.ConnectivityException, e: pass
def pose_cb(ps): m_f, frame = tfu.posestamped_as_matrix(ps) m_o1 = tfu.transform('/omni1', frame, listener) * m_f ee_point = np.matrix(tr.translation_from_matrix(m_o1)).T center = np.matrix([-.10, 0, .30]).T dif = 30*(center - ee_point) #force_dir = dif / np.linalg.norm(dif) force_o1 = dif #force_dir * np.sum(np.power(dif, 2)) force_s = tfu.transform('/omni1_sensable', '/omni1', listener) * np.row_stack((force_o1, np.matrix([1.]))) print np.linalg.norm(center - ee_point) wr = Wrench() wr.force.x = force_s[0] wr.force.y = force_s[1] wr.force.z = force_s[2] wpub.publish(wr)
def publish_relative_frames2(self, data): tmp_dict = {} for segment in data.segments: if(segment.is_quaternion): tmp_dict[segment.id] = (segment.position,segment.quat,self.tr.fromTranslationRotation(segment.position,segment.quat)) else: tmp_dict[segment.id] = (segment.position,segment.euler) for idx in tmp_dict.keys(): p_idx = xsens_client.get_segment_parent_id(idx) child_data = tmp_dict[idx] if(p_idx==-1): continue elif(p_idx==0): helper = tft.quaternion_about_axis(1,(-1,0,0)) new_quat = tft.quaternion_multiply(tft.quaternion_inverse(helper),child_data[1])#if(segment.is_quaternion): TODO Handle Euler #self.sendTransform(child_data[0], self.sendTransform(child_data[0], child_data[1], #(1.0,0,0,0),#FIXME rospy.Time.now(), xsens_client.get_segment_name(idx), self.ref_frame) elif(p_idx>0): parent_data = tmp_dict[p_idx] new_quat = tft.quaternion_multiply(tft.quaternion_inverse(parent_data[1]),child_data[1]) tmp_trans = (parent_data[0][0] - child_data[0][0],parent_data[0][1] - child_data[0][1],parent_data[0][2] - child_data[0][2]) new_trans = qv_mult(parent_data[1],tmp_trans) self.sendTransform( new_trans, new_quat, rospy.Time.now(), xsens_client.get_segment_name(idx), xsens_client.get_segment_name(p_idx)) else: parent_data = tmp_dict[p_idx] this_data = np.multiply(tft.inverse_matrix(child_data[2]) , parent_data[2]) self.sendTransform( tft.translation_from_matrix(this_data), tft.quaternion_from_matrix(this_data), rospy.Time.now(), xsens_client.get_segment_name(idx), xsens_client.get_segment_name(p_idx))
def change_frame(box): listener = tf.TransformListener() print "frame %s" % box.header.frame_id try: now = rospy.Time(0) listener.waitForTransform(box.header.frame_id, 'kinfu_origin', now, rospy.Duration(2.0)) (trans,rot) = listener.lookupTransform('kinfu_origin', box.header.frame_id, now) mat44 = numpy.dot(transformations.translation_matrix(trans), transformations.quaternion_matrix(rot)) pose44 = numpy.dot(tf.listener.xyz_to_mat44(box.pose.position), tf.listener.xyzw_to_mat44(box.pose.orientation)) txpose = numpy.dot(mat44, pose44) xyz = tuple(transformations.translation_from_matrix(txpose))[:3] quat = tuple(transformations.quaternion_from_matrix(txpose)) # assemble return value PoseStampe box.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat)) box.header.frame_id = "kinfu_origin" except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e: print "tf error: %s" % e return
def get_current_pose(self): frame_described_in = str(self.frameline.text()) left = ('Left' == str(self.arm_box.currentText())) right = False if not left: right = True arm_tip_frame = LinearMoveTool.RIGHT_TIP else: arm_tip_frame = LinearMoveTool.LEFT_TIP self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Time(), rospy.Duration(2.)) p_arm = tfu.tf_as_matrix(self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0))) * tr.identity_matrix() trans, rotation = tr.translation_from_matrix(p_arm), tr.quaternion_from_matrix(p_arm) for value, vr in zip(trans, [self.xline, self.yline, self.zline]): vr.setText(str(value)) for value, vr in zip(tr.euler_from_quaternion(rotation), [self.phi_line, self.theta_line, self.psi_line]): vr.setText(str(np.degrees(value)))
def calc_transformation(self, name, relative_to=None): calc_from_joint = False if relative_to: if relative_to in self.urdf.link_map: parent_link_name = relative_to elif relative_to in self.urdf.joint_map: parent_link_name = self.urdf.joint_map[name].parent calc_from_joint = True else: parent_link_name = self.urdf.get_root() calc_to_joint = False if name in self.urdf.link_map: child_link_name = name elif name in self.urdf.joint_map: child_link_name = self.urdf.joint_map[name].child calc_to_joint = True chains = self.urdf.get_chain(parent_link_name, child_link_name, joints=True, links=True) if calc_from_joint: chains = chains[1:] if calc_to_joint: chains = chains[:-1] poses = [] for name in chains: if name in self.urdf.joint_map: joint = self.urdf.joint_map[name] if joint.origin is not None: poses.append(joint.origin) elif name in self.urdf.link_map: link = self.urdf.link_map[name] if link.visual is not None and link.visual.origin is not None: poses.append(link.visual.origin) m = np.dot(T.translation_matrix((0,0,0)), T.euler_matrix(0,0,0)) for p in poses: n = np.dot(T.translation_matrix(tuple(p.xyz)), T.euler_matrix(*tuple(p.rpy))) m = np.dot(m, n) t = T.translation_from_matrix(m) q = T.quaternion_from_matrix(m) return tuple(t), (q[3], q[0], q[1], q[2])
def get_trans_to_trans(trans1, rot1, trans2, rot2): quat_mat1 = transformations.quaternion_matrix(rot1) quat_mat2 = transformations.quaternion_matrix(rot2) trans_mat1 = transformations.translation_matrix(trans1) trans_mat2 = transformations.translation_matrix(trans2) mat1 = transformations.concatenate_matrices(trans_mat1, quat_mat1) mat2 = transformations.concatenate_matrices(trans_mat2, quat_mat2) rel_mat = numpy.linalg.inv(mat1).dot(mat2) rel_trans = transformations.translation_from_matrix(rel_mat) rel_rot = transformations.quaternion_from_matrix(rel_mat) rel_rot = [rel_rot[3], rel_rot[0], rel_rot[1], rel_rot[2]] print "rel_pose: [%f, %f, %f, %f, %f, %f, %f]" % ( rel_trans[0], rel_trans[1], rel_trans[2], rel_rot[0], rel_rot[1], rel_rot[2], rel_rot[3], )
def matrixToPose(matrix): """Converts a Matrix to a Pose message by extracting the translation and rotation specified by the Matrix. Parameters: matrix: a 4x4 transformation matrix """ pos = Pose() T = transformations.translation_from_matrix(matrix) pos.position.x = T[0] pos.position.y = T[1] pos.position.z = T[2] q = transformations.quaternion_from_matrix(matrix) pos.orientation.x = q[0] pos.orientation.y = q[1] pos.orientation.z = q[2] pos.orientation.w = q[3] return pos
def torso_lift_link_T_omni(self, tip_omni, msg_frame): center_T_6 = tfu.transform(self.omni_name+'_center', msg_frame, self.tflistener) tip_center = center_T_6*tip_omni tip_center_t = (np.array(tr.translation_from_matrix(tip_center))*np.array(self.scale_omni_l0)).tolist() tip_center_q = tr.quaternion_from_matrix(tip_center) # z_T_6 = tfu.transform(self.omni_name+'_link0', msg_frame, self.tflistener) # tip_0 = z_T_6*tip_omni # tip_0t = (np.array(tr.translation_from_matrix(tip_0))*np.array(self.scale_omni_l0)).tolist() # tip_0q = tr.quaternion_from_matrix(tip_0) # #Transform into torso frame so we can bound arm workspace tll_T_0 = tfu.transform('/torso_lift_link', self.omni_name + '_center', self.tflistener) tip_torso_mat = tll_T_0 * tfu.tf_as_matrix([tip_center_t, tip_center_q]) tip_tt, tip_tq = tfu.matrix_as_tf(tip_torso_mat) #don't need to bound, arm controller should do this # tip_tt[0] = limit_range(tip_tt[0], self.X_BOUNDS[0], self.X_BOUNDS[1]) self.tip_tt = tip_tt self.tip_tq = tip_tq
def wrench_callback(self, state): # calculating force estimate from position error (does not compensate for motion error due to dynamics) x_err = np.matrix([state.x_err.linear.x, state.x_err.linear.y, state.x_err.linear.z]).T x_dot = np.matrix([state.xd.linear.x, state.xd.linear.y, state.xd.linear.z]).T feedback = -1.0 * self.kp * x_err - self.kd * x_dot # currently the calculated force feedback is published but not to omni, we use the velocity limited state from the controller # wr_ee = [state.F.force.x, state.F.force.y, state.F.force.z] wr_ee = [feedback[0, 0], feedback[1, 0], feedback[2, 0]] # this is a simple FIR filter designed in Matlab to smooth the force estimate shift_right = np.array(self.history[:, 0 : self.FIR.size - 1]) new_col = np.array(wr_ee).reshape((3, 1)) self.history = np.matrix(np.hstack((new_col, shift_right))) wr_ee_filt = self.history * self.FIR # find and use the rotation matrix from wrench to torso df_R_ee = tfu.rotate(self.dest_frame, "torso_lift_link", self.tflistener) * tfu.rotate( "torso_lift_link", self.wrench_frame, self.tflistener ) wr_df = self.force_scaling * np.array( tr.translation_from_matrix( df_R_ee * tfu.translation_matrix([wr_ee_filt[0, 0], wr_ee_filt[1, 0], wr_ee_filt[2, 0]]) ) ) # limiting the max and min force feedback sent to omni wr_df = np.where(wr_df > self.omni_max_limit, self.omni_max_limit, wr_df) wr_df = np.where(wr_df < self.omni_min_limit, self.omni_min_limit, wr_df) wr = Wrench() wr.force.x = wr_df[0, 0] wr.force.y = wr_df[1, 0] wr.force.z = wr_df[2, 0] # publishing of two different wrenches calculated, DELETE first when all finished if self.enable == False: self.filtered_fb.publish(wr) if self.enable == True: self.omni_fb.publish(wr)
def transformBack(tf_xyzquat, pose): T_mat = tfm.concatenate_matrices( tfm.translation_matrix(tf_xyzquat[0:3]), tfm.quaternion_matrix(tf_xyzquat[3:7])) pose_mat = tfm.concatenate_matrices( tfm.translation_matrix(pose[0:3]), tfm.quaternion_matrix(pose[3:7]) ) new_pose_mat = np.dot(pose_mat, tfm.inverse_matrix(T_mat)) return tfm.translation_from_matrix(new_pose_mat).tolist() + tfm.quaternion_from_matrix(new_pose_mat).tolist()
pose.position.y = point[1] pose.position.z = point[2] pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = 0 pose.orientation.w = 1 pos = pose.position ori = pose.orientation mat = np.dot(translation_matrix((pos.x, pos.y, pos.z)), quaternion_matrix((ori.x, ori.y, ori.z, ori.w))) transform_mat = np.dot(translation_matrix((0.017438, 0.110540, -0.003173)), quaternion_matrix((0, 0, 1, 0))) new_mat = np.dot(transform_mat, mat) new_trans = translation_from_matrix(new_mat) new_quat = quaternion_from_matrix(new_mat) new_pose = Pose() new_pose.position.x = new_trans[0] new_pose.position.y = new_trans[1] new_pose.position.z = new_trans[2] new_pose.orientation.x = new_quat[0] new_pose.orientation.y = new_quat[1] new_pose.orientation.z = new_quat[2] new_pose.orientation.w = new_quat[3] util.send_traj_point_marker(marker_pub=marker_pub, pose=new_pose, id=idx + 300, rgba_tuple=rgba_tuple)
def transform_back(pose1, frame): pose1_mat = tfm.concatenate_matrices( tfm.translation_matrix(pose1[0:3]), tfm.quaternion_matrix(pose1[3:7])) frame_mat = tfm.concatenate_matrices( tfm.translation_matrix(frame[0:3]), tfm.quaternion_matrix(frame[3:7]) ) new_pose_mat = np.dot(frame_mat, pose1_mat) return tfm.translation_from_matrix(new_pose_mat).tolist() + tfm.quaternion_from_matrix(new_pose_mat).tolist()
odom_tf.transform.rotation.w) tmat_odom = translation_matrix(odom_trans) qmat_odom = quaternion_matrix(odom_quat) odom_matrix = np.dot(tmat_odom, qmat_odom) height = goal.z # Store/maintain given height # Goal in map coords tmat_goal = translation_matrix((goal.x, goal.y, goal.z)) qmat_goal = quaternion_matrix( quaternion_from_euler(0, 0, math.radians(goal.yaw))) goal_matrix = np.dot(tmat_goal, qmat_goal) # Goal in odom coords final_mat = np.dot(odom_matrix, goal_matrix) trans = translation_from_matrix(final_mat) goal.x = trans[0] goal.y = trans[1] goal.z = height _, _, yaw = euler_from_quaternion( quaternion_from_matrix(final_mat)) goal.yaw = math.degrees(yaw) # goal.header.frame_id = "cf1/odom" print("Transformed goal 'cf1/odom'") print(goal) hover_publisher.publish(goal) goal_pose = goal goal = None
def matrix_to_transform(matrix): t_list = t.translation_from_matrix(matrix) q_list = t.quaternion_from_matrix(matrix) translation = Vector3(t_list[0], t_list[1], t_list[2]) orientation = Quaternion(q_list[0], q_list[1], q_list[2], q_list[3]) return Transform(translation, orientation)
def fk(dhs, qs): Ts = [dh2T(*dh, q=q) for (dh, q) in zip(dhs, qs)] T = reduce(lambda a, b: np.matmul(a, b), Ts) # base_link -> object txn = tx.translation_from_matrix(T) rxn = tx.quaternion_from_matrix(T) return txn, rxn
def estimate(self, t, q, camera_info, target_position=None, occlusion_treshold=0.01, output_viz=True): perspective_timer = cv2.getTickCount() visible_detections = [] rot = quaternion_matrix(q) trans = translation_matrix(t) if target_position is None: target = translation_matrix([0.0, 0.0, 1000.0]) target_position = translation_from_matrix( np.dot(np.dot(trans, rot), target)) view_matrix = p.computeViewMatrix(t, target_position, [0, 0, 1]) width = HumanVisualModel.WIDTH height = HumanVisualModel.HEIGHT projection_matrix = p.computeProjectionMatrixFOV( HumanVisualModel.FOV, HumanVisualModel.ASPECT, HumanVisualModel.CLIPNEAR, HumanVisualModel.CLIPFAR) rendered_width = int(width / 3.0) rendered_height = int(height / 3.0) width_ratio = width / rendered_width height_ratio = height / rendered_height camera_image = p.getCameraImage(rendered_width, rendered_height, viewMatrix=view_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL, projectionMatrix=projection_matrix) rgb_image = cv2.resize(np.array(camera_image[2]), (width, height)) depth_image = np.array(camera_image[3], np.float32).reshape( (rendered_height, rendered_width)) far = HumanVisualModel.CLIPFAR near = HumanVisualModel.CLIPNEAR real_depth_image = far * near / (far - (far - near) * depth_image) if self.use_saliency is not False: saliency_map, saliency_heatmap = self.saliency_estimator.estimate( camera_image[2], real_depth_image) saliency_heatmap_resized = cv2.resize(saliency_heatmap, (width, height)) mask_image = camera_image[4] unique, counts = np.unique(np.array(mask_image).flatten(), return_counts=True) for sim_id, count in zip(unique, counts): if sim_id > 0: cv_mask = np.array(mask_image.copy()) cv_mask[cv_mask != sim_id] = 0 cv_mask[cv_mask == sim_id] = 255 xmin, ymin, w, h = cv2.boundingRect(cv_mask.astype(np.uint8)) visible_area = w * h + 1 screen_area = rendered_width * rendered_height + 1 if screen_area - visible_area == 0: confidence = 1.0 else: confidence = visible_area / float(screen_area - visible_area) #TODO compute occlusion score as a ratio between visible 2d bbox and projected 2d bbox areas depth = real_depth_image[int(ymin + h / 2.0)][int(xmin + w / 2.0)] xmin = int(xmin * width_ratio) ymin = int(ymin * height_ratio) w = int(w * width_ratio) h = int(h * height_ratio) id = self.simulator.reverse_entity_id_map[sim_id] if confidence > occlusion_treshold: det = Detection(int(xmin), int(ymin), int(xmin + w), int(ymin + h), id, confidence, depth=depth) visible_detections.append(det) # for subject_detection in visible_detections: # for object_detection in visible_detections: # if subject_detection != object_detection: # pass #TODO create inference batch for egocentric relation detection perspective_fps = cv2.getTickFrequency() / (cv2.getTickCount() - perspective_timer) if output_viz is True: viz_frame = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR) if self.use_saliency is not False: viz_frame = cv2.addWeighted(saliency_heatmap_resized, 0.4, viz_frame, 0.7, 0) cv2.rectangle(viz_frame, (0, 0), (250, 40), (200, 200, 200), -1) perspective_fps_str = "Perspective taking fps: {:0.1f}hz".format( perspective_fps) cv2.putText(viz_frame, "Nb detections: {}".format(len(visible_detections)), (5, 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) cv2.putText(viz_frame, perspective_fps_str, (5, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) for detection in visible_detections: detection.draw(viz_frame, (0, 200, 0)) return rgb_image, real_depth_image, visible_detections, viz_frame else: return rgb_image, real_depth_image, visible_detections
def matrixToPose(matrix): t = tuple(translation_from_matrix(matrix)) q = tuple(quaternion_from_matrix(matrix)) return Pose(Point(t[0], t[1], t[2]), Quaternion(q[0], q[1], q[2], q[3]))
def __get_cartesian_from_matrix(cls, h_matrix): # Returns a list of [roll, pitch, yaw, X, Y, Z] euler_from_matrix = list(TF.euler_from_matrix(h_matrix)) translation_from_matrix = list(TF.translation_from_matrix(h_matrix)) return euler_from_matrix + translation_from_matrix
def getPositionDistance(self): t = translation_from_matrix(self._matrix) return sqrt(pow(t[0], 2) + pow(t[1], 2) + pow(t[2], 2))
def getTranslation(self): return translation_from_matrix(self._matrix)
def callback(self, msg): tag_infos = [] new_tag_data = AprilTagsWithInfos() # Load tag detections message for detection in msg.detections: # ------ start tag info processing new_info = TagInfo() # Can use id 1 as long as no bundles are used new_info.id = int(detection.tag_id) id_info = self.tags_dict[new_info.id] # rospy.loginfo("[%s] report detected id=[%s] for april tag!",self.node_name,new_info.id) # Check yaml file to fill in ID-specific information new_info.tag_type = self.sign_types[id_info["tag_type"]] if new_info.tag_type == self.info.S_NAME: new_info.street_name = id_info["street_name"] elif new_info.tag_type == self.info.SIGN: new_info.traffic_sign_type = self.traffic_sign_types[ id_info["traffic_sign_type"]] # publish for FSM # parking apriltag event msg_parking = BoolStamped() msg_parking.header.stamp = rospy.Time(0) if new_info.traffic_sign_type == TagInfo.PARKING: msg_parking.data = True else: msg_parking.data = False self.pub_postPros_parking.publish(msg_parking) # intersection apriltag event msg_intersection = BoolStamped() msg_intersection.header.stamp = rospy.Time(0) if ((new_info.traffic_sign_type == TagInfo.FOUR_WAY) or (new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT) or (new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT) or (new_info.traffic_sign_type == TagInfo.T_INTERSECTION)): msg_intersection.data = True else: msg_intersection.data = False self.pub_postPros_intersection.publish(msg_intersection) elif new_info.tag_type == self.info.VEHICLE: new_info.vehicle_name = id_info["vehicle_name"] # TODO: Implement location more than just a float like it is now. # location is now 0.0 if no location is set which is probably not that smart if self.loc == 226: l = id_info["location_226"] if l is not None: new_info.location = l elif self.loc == 316: l = id_info["location_316"] if l is not None: new_info.location = l tag_infos.append(new_info) # --- end tag info processing # Define the transforms veh_t_camxout = tr.translation_matrix( (self.camera_x, self.camera_y, self.camera_z)) veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180, 0, "rxyz") veh_T_camxout = tr.concatenate_matrices( veh_t_camxout, veh_R_camxout) # 4x4 Homogeneous Transform Matrix camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2, "rzyx") veh_T_camzout = tr.concatenate_matrices(veh_T_camxout, camxout_T_camzout) tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2, "rxyz") # Load translation trans = detection.transform.translation rot = detection.transform.rotation header = Header() header.stamp = rospy.Time.now() camzout_t_tagzout = tr.translation_matrix( (trans.x * self.scale_x, trans.y * self.scale_y, trans.z * self.scale_z)) camzout_R_tagzout = tr.quaternion_matrix( (rot.x, rot.y, rot.z, rot.w)) camzout_T_tagzout = tr.concatenate_matrices( camzout_t_tagzout, camzout_R_tagzout) veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout, camzout_T_tagzout, tagzout_T_tagxout) # Overwrite transformed value (trans.x, trans.y, trans.z) = tr.translation_from_matrix(veh_T_tagxout) (rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_matrix(veh_T_tagxout) new_tag_data.detections.append(detection) new_tag_data.infos = tag_infos # Publish Message self.pub_postPros.publish(new_tag_data)
#/map-/base_footprint a0 = [0, 0, 1] b0 = [0.0, 0.0, 0.0, 1] #/odom-/base_footprint a1 = [0, 0, 1] b1 = [0.0, 0.0, 0.0, 1] c0 = np.dot(transformations.translation_matrix(a0), transformations.quaternion_matrix(b0)) c1 = np.linalg.inv(c0) mat44 = np.dot(transformations.translation_matrix(a1), transformations.quaternion_matrix(b1)) # txpose is the new pose in target_frame as a 4x4 txpose = np.dot(mat44, c1) # xyz and quat are txpose's position and orientation txpose = np.linalg.inv(txpose) xyz = tuple(transformations.translation_from_matrix(txpose))[:3] quat = tuple(transformations.quaternion_from_matrix(txpose)) print xyz print quat abc = np.dot(transformations.translation_matrix([0, 0, 0]), transformations.quaternion_matrix(quat)) print abc
def callback(self, pose_ws, transform_cs, c_plane_array): # convert messages to numpy G_ws = poseMsgToMatrix(pose_ws.pose) G_cs = transformMsgToMatrix(transform_cs.transform) # initialization if not self._is_init: # save for next iteration self._G_ws = G_ws self._G_cs = G_cs # anchor starting point t0 = transformations.translation_from_matrix(G_ws) prior_factor = factor.PriorFactor(self._point_var, np.eye(3), t0, 1e-6*np.ones(3)) self._fg.add_factor(prior_factor) self._point_stamp_map[self._point_var] = pose_ws.header.stamp self._is_init = True return # frame-to-frame translation G_ss = np.dot(np.linalg.inv(self._G_ws), G_ws) t_s = transformations.translation_from_matrix(G_ss) # generate odometry factor point_var = variable.PointVariable(3) odom_factor = factor.OdometryFactor(self._point_var, point_var, self._G_cs[:3, :3], t_s, self.sigma_t*np.ones(3)) # generate observation factors obsv_factors = [] for plane in c_plane_array.planes: plane_var = variable.LandmarkVariable(1, plane.label.data, plane.intensity.data) v_c = np.array([[plane.plane.coef[0], plane.plane.coef[1], plane.plane.coef[2]]]) d_c = -plane.plane.coef[3] plane_var.facing = -1 if d_c > 0 else 1 plane_factor = factor.ObservationFactor(point_var, plane_var, v_c, d_c, np.array([self.sigma_d])) obsv_factors.append(plane_factor) # bookkeeping self._var_id_map[plane_var] = plane.id.data if plane.label.data not in self._label_orientation_map: self._label_orientation_map[plane.label.data] = v_c # insert into factorgraph factor_list = [odom_factor] + obsv_factors self._lock.acquire(True) for f in factor_list: self._fg.add_factor(f) self._lock.release() # save for next iteration self._G_ws = G_ws self._G_cs = G_cs self._point_var = point_var # save point timestamp self._point_stamp_map[point_var] = pose_ws.header.stamp
def mat2poselist(mat): pos = tfm.translation_from_matrix(mat) quat = tfm.quaternion_from_matrix(mat) return pos.tolist() + quat.tolist()
def check_const_mating(self, ref_consts, move_consts, tr, quat): tf_mat = utils.get_tf_matrix(tr, quat) for ref_const, move_const in zip(ref_consts, move_consts): ref_const_copy = copy.deepcopy(ref_const) move_const_copy = copy.deepcopy(move_const) move_entry = move_const_copy["entryCoordinate"] move_end = move_const_copy["endCoordinate"] move_const_copy["entryCoordinate"] = tf.concatenate_matrices( tf_mat, move_entry) move_const_copy["endCoordinate"] = tf.concatenate_matrices( tf_mat, move_end) ref_type = ref_const["type"] move_type = move_const["type"] if ref_type == "hole": hole_const = ref_const_copy pin_const = move_const_copy else: hole_const = move_const_copy pin_const = ref_const_copy hole_entry = hole_const["entryCoordinate"] hole_end = hole_const["endCoordinate"] pin_entry = pin_const["entryCoordinate"] pin_end = pin_const["endCoordinate"] hole_entry_tr = tf.translation_from_matrix(hole_entry) hole_end_tr = tf.translation_from_matrix(hole_end) pin_entry_tr = tf.translation_from_matrix(pin_entry) pin_end_tr = tf.translation_from_matrix(pin_end) ref_axis = tf.unit_vector((hole_end_tr - hole_entry_tr).round(6)) eps = 1e-03 ref_axis[np.abs(ref_axis) < eps] = 0 inter_entry_diff = (hole_entry_tr - pin_entry_tr).round(6) #inter_entry_diff[np.abs(inter_entry_diff) < eps] = 0 inter_end_diff = (hole_end_tr - pin_end_tr).round(6) #inter_end_diff[np.abs(inter_end_diff) < eps] = 0 entry_end_diff = (pin_end_tr - hole_entry_tr).round(6) #entry_end_diff[np.abs(entry_end_diff) < eps] = 0 print("\n---") print("parent: {}, child: {}".format(ref_const["name"], move_const["name"])) print(inter_entry_diff) print(inter_end_diff) print(entry_end_diff) '''if np.linalg.norm(inter_entry_diff) == 0. or np.array_equal(np.sign(inter_entry_diff), ref_axis): pass else: print("1") return False if pin_const["feature"] == "screw": continue else: if np.array_equal(np.sign(entry_end_diff), ref_axis): pass else: print("2") return False if np.linalg.norm(inter_end_diff) == 0. or np.array_equal(np.sign(inter_end_diff), ref_axis): pass else: print("3") return False''' return True
def _adjust_camera_position(self): """ Recalculate position of the camera to match robot frames with markers """ if not self._is_robot_camera_frames_of_the_same_figure(): return positions = [] for i in range(len(self._camera_markers_ids)): transformation = self.get_recent_transformation( self._camera_frame, self._camera_markers_ids[i]) point = TransformationManager.point_from_tf(transformation) positions.append(point) markers_positions = numpy.array(positions) positions = [] for i in range(len(self._marker_holder_frames)): transformation = self.get_recent_filtered_transformation( self._camera_frame, self._marker_holder_frames[i]) point = TransformationManager.point_from_tf(transformation) positions.append(point) robot_marker_holders_positions = numpy.array(positions) if not numpy.allclose(robot_marker_holders_positions, markers_positions, atol=self._distance_error_tolerance): affine_transformation_matrix = superimposition_matrix( markers_positions.T, robot_marker_holders_positions.T) translation = translation_from_matrix(affine_transformation_matrix) needed_rotation = quaternion_from_matrix( affine_transformation_matrix) euler_angles = euler_from_quaternion(needed_rotation) apply_translation = not numpy.allclose( translation, numpy.zeros(3), atol=self._distance_error_tolerance) apply_rotation = not numpy.allclose( euler_angles, numpy.zeros(3), atol=self._angle_error_tolerance) if apply_translation or apply_rotation: rospy.loginfo("Applying camera correction") if apply_translation: rospy.loginfo(" Translation => " + str(translation)) self._camera_pose.transform.translation.x += translation[0] self._camera_pose.transform.translation.y += translation[1] self._camera_pose.transform.translation.z += translation[2] if apply_rotation: rospy.loginfo(" Rotation => " + str(needed_rotation)) current_rotation = [self._camera_pose.transform.rotation.x, self._camera_pose.transform.rotation.y, self._camera_pose.transform.rotation.z, self._camera_pose.transform.rotation.w] new_rotation = quaternion_multiply(current_rotation, needed_rotation) self._camera_pose.transform.rotation.x = new_rotation[0] self._camera_pose.transform.rotation.y = new_rotation[1] self._camera_pose.transform.rotation.z = new_rotation[2] self._camera_pose.transform.rotation.w = new_rotation[3]
def get_AsmPose_by_HolePin(self, ref_obj, ref_const_names, move_obj, move_const_names): target_tr = np.array([0, 0, 0]) target_quat = np.array([0, 0, 0, 1]) ref_axis = np.array([0, 0, -1]) criterion = [] success = False ref_consts = [ref_obj.assemConsts[const] for const \ in ref_const_names] move_consts = [move_obj.assemConsts[const] for const \ in move_const_names] criterion = [self.HolePin_criterion(ref, move) for (ref, move) \ in zip(ref_consts, move_consts)] if False in criterion: pprint(ref_consts) pprint(move_consts) rospy.logwarn( "Hole and pin's specs are not satispying given citerion") pass else: ref_coors = [const[cri+"Coordinate"] for (const, cri) \ in zip(ref_consts, criterion)] ref_quats = [tf.quaternion_from_matrix(coor) for coor in ref_coors] ref_trs = [tf.translation_from_matrix(coor) for coor in ref_coors] ref_zs = [utils.get_transformed_zAxis(quat) for quat in ref_quats] ref_z_diffs = [ utils.zAxis_difference(ref_zs[0], zAxis) for zAxis in ref_zs ] ref_tr_diffs = [tr - ref_trs[0] for tr in ref_trs] move_coors = [const[cri+"Coordinate"] for (const, cri) \ in zip(move_consts, criterion)] move_quats = [ tf.quaternion_from_matrix(coor) for coor in move_coors ] move_trs = [ tf.translation_from_matrix(coor) for coor in move_coors ] move_zs = [ utils.get_transformed_zAxis(quat) for quat in move_quats ] move_z_diffs = [ utils.zAxis_difference(move_zs[0], zAxis) for zAxis in move_zs ] move_tr_diffs = [tr - move_trs[0] for tr in move_trs] is_same_z_diffs = [np.allclose(ref_z_diff, move_z_diff, rtol=0.005, atol=0.005) \ for (ref_z_diff, move_z_diff) in zip(ref_z_diffs, move_z_diffs)] if not is_same_z_diffs: print(ref_z_diffs) print(move_z_diffs) print(is_same_z_diffs) rospy.logwarn( "Target hole and pin's direction is not proper for assembly!" ) pass else: if len(ref_consts) > 1: move_quat_inv1 = tf.quaternion_inverse(move_quats[0]) temp_quat = tf.quaternion_multiply(ref_quats[0], move_quat_inv1) ref_axis = ref_zs[0] move2_tr_from_ref1_temp = \ utils.get_translated_origin([0,0,0], temp_quat, move_tr_diffs[1])[:3, 3] move2_tr_from_ref1_temp_re = \ utils.get_translated_origin([0,0,0], tf.quaternion_inverse(ref_quats[0]), move2_tr_from_ref1_temp)[:3, 3] ref2_tr_from_ref1_temp = \ utils.get_translated_origin([0,0,0], tf.quaternion_inverse(ref_quats[0]), ref_tr_diffs[1])[:3, 3] grad_move2_from_ref1_temp = \ np.arctan2(move2_tr_from_ref1_temp_re[1], move2_tr_from_ref1_temp_re[0]) grad_ref2_from_ref1_temp = \ np.arctan2(ref2_tr_from_ref1_temp[1], ref2_tr_from_ref1_temp[0]) temp_grad1 = tf.quaternion_about_axis( grad_move2_from_ref1_temp, ref_axis) temp_grad1_inv = tf.quaternion_inverse(temp_grad1) temp_grad2 = tf.quaternion_about_axis( grad_ref2_from_ref1_temp, ref_axis) target_quat = tf.quaternion_multiply( temp_grad1_inv, temp_quat) target_quat = tf.quaternion_multiply( temp_grad2, target_quat) target_tr = utils.get_translated_origin( ref_trs[0], target_quat, -move_trs[0])[:3, 3] else: ref_axis = ref_zs[0] move_quat_inv = tf.quaternion_inverse(move_quats[0]) target_quat = tf.quaternion_multiply( ref_quats[0], move_quat_inv) target_tr = utils.get_translated_origin( ref_trs[0], target_quat, -move_trs[0])[:3, 3] if "pin" in ref_const_names[0]: ref_axis = -ref_zs[0] else: ref_axis = ref_zs[0] edit_quats = [tf.quaternion_multiply(target_quat, move_quat) \ for move_quat in move_quats] edit_zs = [ utils.get_transformed_zAxis(quat) for quat in edit_quats ] edit_quat_mat = tf.quaternion_matrix(target_quat) edit_coors = [tf.concatenate_matrices(edit_quat_mat, coor) \ for coor in move_coors] edit_trs = [ tf.translation_from_matrix(coor) for coor in edit_coors ] edit_tr_diffs = [tr - edit_trs[0] for tr in edit_trs] edit_tr_min = self.get_min_vector_from_ref( edit_zs, edit_trs, edit_tr_diffs) ref_tr_min = self.get_min_vector_from_ref( ref_zs, ref_trs, ref_tr_diffs) is_same_arange = [np.allclose(ref, move, rtol=0.05, atol=0.05) \ for (ref, move) in zip(ref_tr_min, edit_tr_min)] arange_success = all(is_same_arange) stuck_success = self.check_const_mating( ref_consts, move_consts, target_tr, target_quat) success = arange_success and stuck_success return target_tr, target_quat, ref_axis, criterion, success
def set_pose(self, joint_group, position, rpy=None, task_duration=7.0, do_wait=True, ref_frame_name=None): ''' @deprecated: Use set_pose_target (from MoveGroupCommander) directly. Accept pose defined by position and RPY in Cartesian format. @type joint_group: str @type position: [float] @param position: x, y, z. @type rpy: [float] @param rpy: If None, keep the current orientation by using MoveGroupCommander.set_position_target. See: 'http://moveit.ros.org/doxygen/' + 'classmoveit__commander_1_1move__group_1_1' + 'MoveGroupCommander.html#acfe2220fd85eeb0a971c51353e437753' @param ref_frame_name: reference frame for target pose, i.e. "LARM_JOINT5_Link". ''' # convert to tuple to list position = list(position) if not rpy is None: rpy = list(rpy) # # Check if MoveGroup is instantiated. if not self.MG_LARM or not self.MG_RARM: try: self._init_moveit_commanders() except RuntimeError as e: rospy.logerr(self._MSG_NO_MOVEGROUP_FOUND) raise e # Locally assign the specified MoveGroup movegr = None if Constant.GRNAME_LEFT_ARM == joint_group: movegr = self.MG_LARM elif Constant.GRNAME_RIGHT_ARM == joint_group: movegr = self.MG_RARM else: rospy.logerr('joint_group must be either %s, %s or %s' % (Constant.GRNAME_LEFT_ARM, Constant.GRNAME_RIGHT_ARM, Constant.GRNAME_BOTH_ARMS)) return # set reference frame if ref_frame_name: ref_pose = movegr.get_current_pose(ref_frame_name).pose ref_mat = compose_matrix(translate=[ ref_pose.position.x, ref_pose.position.y, ref_pose.position.z ], angles=list( euler_from_quaternion([ ref_pose.orientation.x, ref_pose.orientation.y, ref_pose.orientation.z, ref_pose.orientation.w ]))) target_mat = numpy.dot( ref_mat, compose_matrix(translate=position, angles=rpy or [0, 0, 0])) position = list(translation_from_matrix(target_mat)) rpy = list(euler_from_matrix(target_mat)) # If no RPY specified, give position and return the method. if not rpy: try: movegr.set_position_target(position) except MoveItCommanderException as e: rospy.logerr(str(e)) (movegr.go(do_wait) or movegr.go(do_wait) or rospy.logerr( 'MoveGroup.go fails; jointgr={}'.format(joint_group))) return # Not necessary to convert from rpy to quaternion, since # MoveGroupCommander.set_pose_target can take rpy format too. pose = copy.deepcopy(position) pose.extend(rpy) rospy.loginfo('setpose jntgr={} mvgr={} pose={} posi={} rpy={}'.format( joint_group, movegr, pose, position, rpy)) try: movegr.set_pose_target(pose) except MoveItCommanderException as e: rospy.logerr(str(e)) except Exception as e: rospy.logerr(str(e)) (movegr.go(do_wait) or movegr.go(do_wait) or rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group)))
def numpy_to_pose(P): xyz = tuple(transformations.translation_from_matrix(P))[:3] quat = tuple(transformations.quaternion_from_matrix(P)) return geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat))
return res_1.x if __name__ == '__main__': color1 = (0,255,255) color2 = (0,0,255) color3 = (255,0,255) # x0_ext_old = [1.08592196e-01, -5.96469288e-01, 6.09164354e-01] + # [9.05378371e-01, 3.06042346e-02, -5.82887034e-02, -4.19470873e-01]) # viewer x0_ext_old = [7.16834068e-01, -7.68849430e-02, 3.78838750e-01] + [6.89344221e-01, 6.44350485e-01, -2.20748124e-01, -2.46753445e-01] # observer transform = tfm.concatenate_matrices(tfm.translation_matrix(x0_ext_old[0:3]), tfm.quaternion_matrix(x0_ext_old[3:7])) inversed_transform = tfm.inverse_matrix(transform) translation = tfm.translation_from_matrix(inversed_transform) quaternion = tfm.quaternion_from_matrix(inversed_transform) x0_ext = translation.tolist() + quat_to_rod(quaternion.tolist()) if sys.argv[1] == 'observer': if sys.argv[3] == '640': x0_int = [617.605, 617.605, 305.776, 238.914, 0.0,0.0,0.0,0.0,0.0] # observer elif sys.argv[3] == '1080': x0_int = [1389.61, 1389.61, 927.997, 537.558, 0.0,0.0,0.0,0.0,0.0] # observer else: if sys.argv[3] == '640': x0_int = [618.337, 618.337, 310.987, 236.15, 0.0,0.0,0.0,0.0,0.0] # viewer elif sys.argv[3] == '1080': x0_int = [1391.26, 1391.26, 939.721, 531.336, 0.0,0.0,0.0,0.0,0.0] # viewer
def dh_2_rpy(table_name, config_file): lines_read = 0 #nie bierzemy pod uwage 1 wiersza with open(join('config', table_name), 'r') as f: reader = csv.DictReader(f, delimiter=' ') for row in reader: for key in list(row): table[key].append(float(row[key])) lines_read+=1 print(table) print(lines_read) assert(lines_read == 3) roll = [] pitch = [] yaw = [] xes = [] yes = [] zes = [] x_axis = (1, 0, 0) z_axis = (0, 0, 1) params = [] link_len = [] for i in range(0, lines_read): tz = translation_matrix((0, 0, table['d'][i])) rz = rotation_matrix(table['theta'][i], z_axis) tx = translation_matrix((table['a'][i], 0, 0)) rx = rotation_matrix(table['alpha'][i], x_axis) params.append({'theta': table['theta'][i], 'alpha': table['alpha'][i], 'a': table['a'][i], 'd': table['d'][i]}) #params = table dh_matrix = concatenate_matrices(tx, rx, tz, rz) (r, p, y) = euler_from_matrix(dh_matrix) (x_, y_, z_) = translation_from_matrix(dh_matrix) roll.append(r) pitch.append(p) yaw.append(y) xes.append(x_) yes.append(y_) zes.append(z_) link_len.append(table['d'][i]) #params = [] with open(join('config', config_file), 'w+') as f: for i in range(0, lines_read): #joint = {} f.write("roll%d: %.2f\n" % ((i+1), roll[i])) #joint['roll'] = float(roll[i]) f.write("pitch%d: %.2f\n" % ((i+1), pitch[i])) #joint['pitch'] = float(pitch[i]) f.write("yaw%d: %.2f\n" % ((i+1), yaw[i])) #joint['yaw'] = float(yaw[i]) f.write("x%d: %.2f\n" % ((i+1), xes[i])) #joint['x'] = float(xes[i]) f.write("y%d: %.2f\n" % ((i+1), yes[i])) #joint['y'] = float(yes[i]) f.write("z%d: %.2f\n" % ((i+1), zes[i])) #joint['z'] = float(zes[i]) f.write("link%d_length: %.2f\n" % ((i+1), float(link_len[i]))) #params.append(joint) return params
def prepare_corner_grasp_parameter(ec_frame, selected_wall_names, objects, current_object_idx, object_params, ifco_in_base_transform, params): # hand pose above and behind the object pre_approach_transform = params['pre_approach_transform'] corner_frame = np.copy(ec_frame) print("Prepare Corner: ", pu.tf_dbg_call_to_string(corner_frame, "prepare")) used_ec_frame, corner_frame_alpha_zero = GraspFrameRecipes.get_derived_corner_grasp_frames( corner_frame, object_params['frame']) pre_approach_pose = used_ec_frame.dot(params['hand_transform'].dot( pre_approach_transform)) # TODO order of hand and pre_approach # down_dist = params['down_dist'] # dist lower than ifco bottom: behavior of the high level planner # dist = z difference to ifco bottom minus hand frame offset (dist from hand frame to collision point) # (more realistic behavior since we have a force threshold when going down to the bottom) bounded_down_dist = pre_approach_pose[2, 3] - ifco_in_base_transform[2, 3] hand_frame_to_bottom_offset = 0.07 # 7cm TODO maybe move to handarm_parameters.py bounded_down_dist = min(params['down_dist'], bounded_down_dist - hand_frame_to_bottom_offset) # goal pose for go down movement go_down_pose = tra.translation_matrix([0, 0, -bounded_down_dist ]).dot(pre_approach_pose) # pose after lifting. This is somewhat fake, since the real go_down_pose will be determined by # the FT-Switch during go_down and the actual lifted distance by the TimeSwitch (or a pose switch in case # the robot allows precise small movements) TODO better solution? fake_lift_up_dist = np.min([params['lift_dist'], 0.01]) # 1cm corrective_lift_pose = tra.translation_matrix([0, 0, fake_lift_up_dist ]).dot(go_down_pose) sliding_dist = params['sliding_dist'] wall_dir = tra.translation_matrix([0, 0, -sliding_dist]) # slide direction is given by the corner_frame_alpha_zero wall_dir[:3, 3] = corner_frame_alpha_zero[:3, :3].dot(wall_dir[:3, 3]) slide_to_wall_pose = wall_dir.dot(corrective_lift_pose) # now project it into the wall plane! z_projection = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1]]) to_wall_plane_transform = corner_frame_alpha_zero.dot( z_projection.dot( tra.inverse_matrix(corner_frame_alpha_zero).dot( slide_to_wall_pose))) slide_to_wall_pose[:3, 3] = tra.translation_from_matrix( to_wall_plane_transform) checked_motions = [ 'pre_approach', 'go_down', 'corrective_lift', 'slide_to_wall' ] goals = [ pre_approach_pose, go_down_pose, corrective_lift_pose, slide_to_wall_pose ] # Take orientation of object but translation of pre grasp pose pre_grasp_pos_manifold = np.copy(object_params['frame']) pre_grasp_pos_manifold[:3, 3] = tra.translation_from_matrix(pre_approach_pose) slide_pos_manifold = np.copy(slide_to_wall_pose) goal_manifold_frames = { 'pre_approach': pre_grasp_pos_manifold, # Use object frame for sampling 'go_down': np.copy(go_down_pose), 'corrective_lift': np.copy(corrective_lift_pose), # should always be the same frame as go_down # TODO use world orientation? # Use wall frame for sampling. Keep in mind that the wall frame has different orientation, than world. 'slide_to_wall': slide_pos_manifold, } goal_manifold_orientations = { # use hand orientation 'pre_approach': tra.quaternion_from_matrix(pre_approach_pose), # Use object orientation 'go_down': tra.quaternion_from_matrix( go_down_pose), # TODO use hand orietation instead? # should always be the same orientation as go_down 'corrective_lift': tra.quaternion_from_matrix(corrective_lift_pose), # use wall orientation 'slide_to_wall': tra.quaternion_from_matrix( corner_frame), # TODO is that the right one? } allowed_collisions = { # 'init_joint': [], # no collisions are allowed during going to pre_grasp pose 'pre_approach': [], # Only allow touching the bottom of the ifco 'go_down': [ AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='bottom', terminating=False), ], 'corrective_lift': [ AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='bottom', terminating=False), ], 'slide_to_wall': [ # Allow all other objects to be touched as well # (since hand will go through them in simulation) AllowedCollision(type=AllowedCollision.BOUNDING_BOX, box_id=obj_idx, terminating=False, required=obj_idx == current_object_idx) for obj_idx in range(0, len(objects)) ] + [ AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name=selected_wall_names[0], terminating=False), AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name=selected_wall_names[1], terminating=False), AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='bottom', terminating=False), ], } return FeasibilityQueryParameters(checked_motions, goals, allowed_collisions, goal_manifold_frames, goal_manifold_orientations)
def xyzquat_from_matrix(matrix): return tfm.translation_from_matrix(matrix).tolist() + tfm.quaternion_from_matrix(matrix).tolist()
def step(self): t = rospy.Time.now() _, _, oldwidth, oldheight = glGetFloatv(GL_VIEWPORT) height = min(oldheight, int(oldwidth / self.aspect + .5)) width = int(self.aspect * height + .5) glViewport(0, 0, width, height) msg = CameraInfo() msg.header.stamp = t msg.header.frame_id = '/' + self.name msg.height = height msg.width = width f = 1 / math.tan(math.radians(self.fovy) / 2) * height / 2 msg.P = [ f, 0, width / 2 - .5, 0, 0, f, height / 2 - .5, 0, 0, 0, 1, 0, ] self.info_pub.publish(msg) glMatrixMode(GL_PROJECTION) glLoadIdentity() perspective(self.fovy, width / height, 0.1) glMatrixMode(GL_MODELVIEW) glLoadIdentity() # rotates into the FLU coordinate system glMultMatrixf([[0., 0., -1., 0.], [-1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]]) # after that, +x is forward, +y is left, and +z is up self.set_pose_func() with GLMatrix: rotate_to_body(self.base_link_body) glcamera_from_body = glGetFloatv(GL_MODELVIEW_MATRIX).T camera_from_body = numpy.array([ # camera from glcamera [1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1], ]).dot(glcamera_from_body) body_from_camera = numpy.linalg.inv(camera_from_body) tf_br.sendTransform( transformations.translation_from_matrix(body_from_camera), transformations.quaternion_from_matrix(body_from_camera), t, "/" + self.name, "/base_link") if not self.image_pub.get_num_connections(): glViewport(0, 0, oldwidth, oldheight) return self.world.draw() x = glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, outputType=None) x = numpy.reshape(x, (height, width, 4)) x = x[::-1] msg = Image() msg.header.stamp = t msg.header.frame_id = '/' + self.name msg.height = height msg.width = width msg.encoding = 'rgba8' msg.is_bigendian = 0 msg.step = width * 4 msg.data = x.tostring() self.image_pub.publish(msg) glViewport(0, 0, oldwidth, oldheight)
def prepare_surface_grasp_parameter(objects, current_object_idx, object_params, params): # use kinematic checks # TODO create proxy; make it a persistent connection? # Code duplication from planner.py TODO put at a shared location # Set the initial pose above the object goal_ = np.copy( object_params['frame']) # TODO: this should be support_surface_frame goal_[:3, 3] = tra.translation_from_matrix(object_params['frame']) goal_ = goal_.dot(params['hand_transform']) # the grasp frame is symmetrical - check which side is nicer to reach # this is a hacky first version for our WAM zflip_transform = tra.rotation_matrix(math.radians(180.0), [0, 0, 1]) if goal_[0][0] < 0: goal_ = goal_.dot(zflip_transform) # hand pose above object pre_grasp_pose = goal_.dot(params['pre_approach_transform']) # down_dist = params['down_dist'] # dist lower than ifco bottom: behavior of the high level planner # dist = z difference to object centroid (both transformations are w.r.t. to world frame # (more realistic behavior since we have to touch the object for a successful grasp) down_dist = pre_grasp_pose[2, 3] - object_params['frame'][ 2, 3] # get z-translation difference # goal pose for go down movement go_down_pose = tra.translation_matrix([0, 0, -down_dist]).dot(pre_grasp_pose) post_grasp_pose = params['post_grasp_transform'].dot( go_down_pose ) # TODO it would be better to allow relative motion as goal frames checked_motions = [ "pre_approach", "go_down" ] # , "post_grasp_rot"] ,go_up, go_drop_off # TODO what about remaining motions? (see wallgrasp) goals = [pre_grasp_pose, go_down_pose] # , post_grasp_pose] # TODO what about using the bounding boxes as for automatic goal manifold calculation? # Take orientation of object but translation of pre grasp pose pre_grasp_pos_manifold = np.copy(object_params['frame']) pre_grasp_pos_manifold[:3, 3] = tra.translation_from_matrix(pre_grasp_pose) goal_manifold_frames = { 'pre_approach': pre_grasp_pos_manifold, # Use object frame for resampling 'go_down': np.copy( object_params['frame']) # TODO change that again to go_down_pose!? } goal_manifold_orientations = { # use hand orientation 'pre_approach': tra.quaternion_from_matrix(pre_grasp_pose), # Use object orientation 'go_down': tra.quaternion_from_matrix(go_down_pose), # tra.quaternion_from_matrix(object_params['frame']) # TODO use hand orietation instead? } # The collisions that are allowed per motion in message format allowed_collisions = { # no collisions are allowed during going to pre_grasp pose 'pre_approach': [], 'go_down': [ AllowedCollision(type=AllowedCollision.BOUNDING_BOX, box_id=current_object_idx, terminating=True, required=True), AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='bottom', terminating=False) ] + [ AllowedCollision(type=AllowedCollision.BOUNDING_BOX, box_id=obj_idx, terminating=False) for obj_idx, o in enumerate(objects) if obj_idx != current_object_idx and params['go_down_allow_touching_other_objects'] ], # TODO also account for the additional object in a way? 'post_grasp_rot': [ AllowedCollision(type=AllowedCollision.BOUNDING_BOX, box_id=current_object_idx, terminating=True), AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='bottom', terminating=False) ] } print("ALLOWED COLLISIONS:", allowed_collisions) return FeasibilityQueryParameters(checked_motions, goals, allowed_collisions, goal_manifold_frames, goal_manifold_orientations)
def matrix_as_tf(mat): return (tr.translation_from_matrix(mat), tr.quaternion_from_matrix(mat))
def create_wall_grasp(object_frame, bounding_box, wall_frame, handarm_params, object_type, pre_grasp_pose=None, alternative_behavior=None): # Get the parameters from the handarm_parameters.py file obj_type_params = {} obj_params = {} if object_type in handarm_params['wall_grasp']: obj_type_params = handarm_params['wall_grasp'][object_type] if 'object' in handarm_params['wall_grasp']: obj_params = handarm_params['wall_grasp']['object'] hand_transform = getParam(obj_type_params, obj_params, 'hand_transform') downward_force = getParam(obj_type_params, obj_params, 'downward_force') wall_force = getParam(obj_type_params, obj_params, 'wall_force') slide_IFCO_speed = getParam(obj_type_params, obj_params, 'slide_speed') pre_approach_transform = getParam(obj_type_params, obj_params, 'pre_approach_transform') scooping_angle_deg = getParam(obj_type_params, obj_params, 'scooping_angle_deg') init_joint_config = handarm_params['init_joint_config'] thumb_pos_preshape = getParam(obj_type_params, obj_params, 'thumb_pos_preshape') post_grasp_transform = getParam(obj_type_params, obj_params, 'post_grasp_transform') rotate_time = handarm_params['rotate_duration'] down_IFCO_speed = handarm_params['down_IFCO_speed'] # Set the twists to use TRIK controller with # Down speed is negative because it is defined on the world frame down_IFCO_twist = np.array([0, 0, -down_IFCO_speed, 0, 0, 0]) # Slide speed is positive because it is defined on the EE frame + rotation of the scooping angle slide_IFCO_twist_matrix = tra.rotation_matrix( math.radians(scooping_angle_deg), [1, 0, 0]).dot(tra.translation_matrix([0, 0, slide_IFCO_speed])) slide_IFCO_twist = np.array([ slide_IFCO_twist_matrix[0, 3], slide_IFCO_twist_matrix[1, 3], slide_IFCO_twist_matrix[2, 3], 0, 0, 0 ]) rviz_frames = [] if pre_grasp_pose is None: # this is the EC frame. It is positioned like object and oriented to the wall ec_frame = np.copy(wall_frame) ec_frame[:3, 3] = tra.translation_from_matrix(object_frame) ec_frame = ec_frame.dot(hand_transform) pre_approach_pose = ec_frame.dot(pre_approach_transform) else: pre_approach_pose = pre_grasp_pose # Rviz debug frames rviz_frames.append(object_frame) rviz_frames.append(pre_approach_pose) # rviz_frames.append(pm.toMatrix(pm.fromMsg(res.reachable_hand_pose))) control_sequence = [] # # 0. Go to initial nice mid-joint configuration # control_sequence.append(ha.JointControlMode(goal = init_joint_config, goal_is_relative = '0', name = 'init', controller_name = 'GoToInitController')) # # 0b. Switch when config is reached # control_sequence.append(ha.JointConfigurationSwitch('init', 'Pregrasp', controller = 'GoToInitController', epsilon = str(math.radians(1.0)))) # 1. Go above the object control_sequence.append( ha.InterpolatedHTransformControlMode(pre_approach_pose, controller_name='GoAboveObject', goal_is_relative='0', name="Pregrasp")) # 1b. Switch when hand reaches the goal pose control_sequence.append( ha.FramePoseSwitch('Pregrasp', 'StayStill', controller='GoAboveObject', epsilon='0.01')) # 2. Go to gravity compensation control_sequence.append( ha.CartesianVelocityControlMode(np.array([0, 0, 0, 0, 0, 0]), controller_name='StayStillCtrl', name="StayStill", reference_frame="EE")) # 2b. Wait for a bit to allow vibrations to attenuate control_sequence.append( ha.TimeSwitch('StayStill', 'softhand_pretension', duration=handarm_params['stay_still_duration'])) # 3. Pretension speed = np.array([20]) thumb_pos = np.array([0, 0, 0]) diff_pos = np.array([0, 0, 15]) thumb_contact_force = np.array([0]) thumb_grasp_force = np.array([0]) diff_contact_force = np.array([0]) diff_grasp_force = np.array([0]) thumb_pretension = np.array([0]) diff_pretension = np.array([15]) force_feedback_ratio = np.array([0]) prox_level = np.array([0]) touch_level = np.array([0]) mode = np.array([0]) command_count = np.array([0]) control_sequence.append( ha.ros_CLASHhandControlMode(goal=np.concatenate( (speed, thumb_pos, diff_pos, thumb_contact_force, thumb_grasp_force, diff_contact_force, diff_grasp_force, thumb_pretension, diff_pretension, force_feedback_ratio, prox_level, touch_level, mode, command_count)), name='softhand_pretension')) # 3b. Switch when hand closing time ends control_sequence.append( ha.TimeSwitch('softhand_pretension', 'GoDown', duration=handarm_params['hand_closing_duration'])) # 4. Go down onto the object/table, in world frame control_sequence.append( ha.CartesianVelocityControlMode(down_IFCO_twist, controller_name='GoDown', name="GoDown", reference_frame="world")) # 4b. Switch when force threshold is exceeded force = np.array([0, 0, downward_force, 0, 0, 0]) control_sequence.append( ha.ForceTorqueSwitch('GoDown', 'CloseBeforeSlide', goal=force, norm_weights=np.array([0, 0, 1, 0, 0, 0]), jump_criterion="THRESH_UPPER_BOUND", goal_is_relative='1', frame_id='world', port='2')) # 5. Close a bit before sliding speed = np.array([30]) thumb_pos = thumb_pos_preshape diff_pos = np.array([10, 15, 0]) thumb_contact_force = np.array([0]) thumb_grasp_force = np.array([0]) diff_contact_force = np.array([0]) diff_grasp_force = np.array([0]) thumb_pretension = np.array([15]) diff_pretension = np.array([15]) force_feedback_ratio = np.array([0]) prox_level = np.array([0]) touch_level = np.array([0]) mode = np.array([0]) command_count = np.array([1]) control_sequence.append( ha.ros_CLASHhandControlMode(goal=np.concatenate( (speed, thumb_pos, diff_pos, thumb_contact_force, thumb_grasp_force, diff_contact_force, diff_grasp_force, thumb_pretension, diff_pretension, force_feedback_ratio, prox_level, touch_level, mode, command_count)), name='CloseBeforeSlide')) # 5b. Time switch control_sequence.append( ha.TimeSwitch('CloseBeforeSlide', 'SlideToWall', duration=handarm_params['hand_closing_duration'])) # 6. Go towards the wall to slide object to wall control_sequence.append( ha.CartesianVelocityControlMode(slide_IFCO_twist, controller_name='SlideToWall', name="SlideToWall", reference_frame="EE")) # 6b. Switch when the f/t sensor is triggered with normal force from wall force = np.array([0, 0, wall_force, 0, 0, 0]) control_sequence.append( ha.ForceTorqueSwitch('SlideToWall', 'softhand_close', 'ForceSwitch', goal=force, norm_weights=np.array([0, 0, 1, 0, 0, 0]), jump_criterion="THRESH_UPPER_BOUND", goal_is_relative='1', frame_id='world', frame=wall_frame, port='2')) # 7. Close the hand speed = np.array([30]) thumb_pos = np.array([0, 50, 30]) diff_pos = np.array([55, 50, 20]) thumb_contact_force = np.array([0]) thumb_grasp_force = np.array([0]) diff_contact_force = np.array([0]) diff_grasp_force = np.array([0]) thumb_pretension = np.array([15]) diff_pretension = np.array([15]) force_feedback_ratio = np.array([0]) prox_level = np.array([0]) touch_level = np.array([0]) mode = np.array([0]) command_count = np.array([1]) control_sequence.append( ha.ros_CLASHhandControlMode(goal=np.concatenate( (speed, thumb_pos, diff_pos, thumb_contact_force, thumb_grasp_force, diff_contact_force, diff_grasp_force, thumb_pretension, diff_pretension, force_feedback_ratio, prox_level, touch_level, mode, command_count)), name='softhand_close')) # 7b. Switch when hand closing time ends control_sequence.append( ha.TimeSwitch('softhand_close', 'PostGraspRotate', duration=handarm_params['hand_closing_duration'])) # 8. Rotate hand after closing and before lifting it up relative to current hand pose control_sequence.append( ha.CartesianVelocityControlMode(post_grasp_transform, controller_name='PostGraspRotate', name='PostGraspRotate', reference_frame='EE')) # 8b. Switch when hand rotated control_sequence.append( ha.TimeSwitch('PostGraspRotate', 'GoUp', duration=rotate_time)) return control_sequence, rviz_frames
def data_cb(self, joint_msg, detection_msgs): try: jorder = ['waist', 'shoulder', 'elbow', 'hand', 'wrist'] j_idx = [joint_msg.name.index(j) for j in jorder] j = [joint_msg.position[i] for i in j_idx] j = np.concatenate((j, [0])) # assume final joint is fixed except Exception as e: rospy.logerr_throttle( 1.0, 'Joint Positions Failed : {} \n {}'.format(e, joint_msg)) Xs = [np.eye(4) for _ in range(self._num_markers)] vis = [False for _ in range(self._num_markers)] if len(detection_msgs.detections) <= 0: return try: for pm in detection_msgs.detections: # pm.pose = pwcs # pm.pose.pose = pwc # pm.pose.pose.pose = p # TODO : technically requires tf.transformPose(...) for robustness. #pose = tf.transformPose( ps = PoseStamped(header=pm.pose.header, pose=pm.pose.pose.pose) # this transform should theoretically be painless + static # Optionally, store the transform beforehand and apply it ps = self._tfl.transformPose('stereo_optical_link', ps) q = ps.pose.orientation p = ps.pose.position m_id = pm.id[0] X = tx.compose_matrix(angles=tx.euler_from_quaternion( [q.x, q.y, q.z, q.w]), translate=[p.x, p.y, p.z]) i = self._m2i[m_id] # transform tag id to index if i >= self._num_markers: continue self._i2m[i] = m_id Xs[i] = X vis[i] = True self._seen[i] = True except Exception as e: rospy.logerr_throttle(1.0, 'TF Failed : {}'.format(e)) Xs = np.float32(Xs) self._data.append((j, Xs, vis)) self._step += 1 rospy.loginfo_throttle( 1.0, 'Current Step : {}; vis : {}'.format(self._step, vis)) T = self._calib.eval_1(j, Xs, vis) # == (1, M, 4, 4) now = rospy.Time.now() m_msg = AprilTagDetectionArray() m_msg.header.frame_id = 'base_link' m_msg.header.stamp = now pv_msg = PoseArray() pv_msg.header.stamp = now pv_msg.header.frame_id = 'base_link' for i in range(self._num_markers): if vis[i]: txn = tx.translation_from_matrix(T[i]) rxn = tx.quaternion_from_matrix(T[i]) msg = AprilTagDetection() msg.id = [self._i2m[i]] msg.size = [0.0] # not really a thing pwcs = msg.pose pwcs.header.frame_id = 'base_link' pwcs.header.stamp = now fill_pose_msg(pwcs.pose.pose, txn, rxn) m_msg.detections.append(msg) pv_msg.poses.append(pwcs.pose.pose) self._pub.publish(m_msg) self._vpub.publish(pv_msg)
def pack_matrix(matrix, time, child, parent): trans = tfs.translation_from_matrix(matrix) rot = tfs.quaternion_from_matrix(matrix) return pack_transrot(trans, rot, time, child, parent)